Rigid constraint tutorial

Rigid constraint tutorial#

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 Rigid object group tutorial 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 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.

Code for create_rigid_constraint.py
  1# ----------------------------------------------------------------------------
  2# Copyright (c) 2021-2026 DexForce Technology Co., Ltd.
  3#
  4# Licensed under the Apache License, Version 2.0 (the "License");
  5# you may not use this file except in compliance with the License.
  6# You may obtain a copy of the License at
  7#
  8#     http://www.apache.org/licenses/LICENSE-2.0
  9#
 10# Unless required by applicable law or agreed to in writing, software
 11# distributed under the License is distributed on an "AS IS" BASIS,
 12# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 13# See the License for the specific language governing permissions and
 14# limitations under the License.
 15# ----------------------------------------------------------------------------
 16
 17"""
 18This script demonstrates how to attach two rigid objects via a fixed constraint,
 19observe the constraint holding their relative pose, and then remove it.
 20"""
 21
 22from __future__ import annotations
 23
 24import argparse
 25import sys
 26
 27from embodichain.lab.sim import SimulationManager, SimulationManagerCfg
 28from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser
 29from embodichain.lab.sim.cfg import (
 30    RigidObjectCfg,
 31    RigidConstraintCfg,
 32    RigidBodyAttributesCfg,
 33    RenderCfg,
 34)
 35from embodichain.lab.sim.shapes import CubeCfg
 36
 37# Number of physics sub-steps per update call.
 38STEPS_PER_UPDATE = 1
 39# Print the relative pose every N update calls.
 40PRINT_EVERY = 20
 41# How long to simulate while attached / detached (in update calls).
 42PHASE_STEPS = 120
 43
 44
 45def main():
 46    """Main function to create and run the constraint tutorial scene."""
 47
 48    # Parse command line arguments (adds --headless, --num_envs, --device, ...).
 49    parser = argparse.ArgumentParser(
 50        description="Attach and detach two cubes via a fixed rigid constraint"
 51    )
 52    add_env_launcher_args_to_parser(parser)
 53    args = parser.parse_args()
 54
 55    # The simulation teardown (``SimulationManager.destroy``) calls ``os._exit``,
 56    # which skips flushing Python's stdout buffer. Line-buffer stdout so every
 57    # ``print`` below is visible even when the script is piped to a file.
 58    sys.stdout.reconfigure(line_buffering=True)
 59
 60    # Configure the simulation.
 61    sim_cfg = SimulationManagerCfg(
 62        width=1920,
 63        height=1080,
 64        headless=args.headless,
 65        physics_dt=1.0 / 100.0,  # Physics timestep (100 Hz)
 66        sim_device=args.device,
 67        render_cfg=RenderCfg(renderer=args.renderer),
 68        num_envs=args.num_envs,
 69        arena_space=3.0,
 70    )
 71
 72    sim = SimulationManager(sim_cfg)
 73
 74    # Shared physics attributes for the two cubes.
 75    physics_attrs = RigidBodyAttributesCfg(
 76        mass=0.2,
 77        dynamic_friction=0.5,
 78        static_friction=0.5,
 79        restitution=0.1,
 80    )
 81
 82    # Add two dynamic cubes to the scene. cube_a starts higher than cube_b so
 83    # that, once detached, the lower cube lands first and the relative pose
 84    # visibly changes (while welded, the constraint holds it constant).
 85    cube_a = sim.add_rigid_object(
 86        cfg=RigidObjectCfg(
 87            uid="cube_a",
 88            shape=CubeCfg(size=[0.16, 0.16, 0.16]),
 89            attrs=physics_attrs,
 90            init_pos=[0.0, 0.0, 1.40],
 91        )
 92    )
 93    cube_b = sim.add_rigid_object(
 94        cfg=RigidObjectCfg(
 95            uid="cube_b",
 96            shape=CubeCfg(size=[0.16, 0.16, 0.16]),
 97            attrs=physics_attrs,
 98            init_pos=[0.0, 0.0, 1.20],
 99        )
100    )
101
102    if sim.is_use_gpu_physics:
103        sim.init_gpu_physics()
104
105    print("[INFO]: Scene setup complete with two cubes (cube_a, cube_b).")
106
107    # --- Phase 1: attach the two cubes with a fixed constraint ---------------
108    # With default (None) local frames the constraint welds the cubes at their
109    # *current* relative pose: local_frame_a defaults to identity and
110    # local_frame_b is computed as inv(pose_B) @ pose_A, so the offset is
111    # preserved rather than the two origins being pulled together.
112    constraint = sim.create_rigid_constraint(
113        cfg=RigidConstraintCfg(
114            name="cube_weld",
115            rigid_object_a_uid="cube_a",
116            rigid_object_b_uid="cube_b",
117        )
118    )
119    print("[INFO]: Created constraint 'cube_weld' between cube_a and cube_b.")
120
121    # Open the viewer (unless --headless) so the welded motion is visible.
122    if not args.headless:
123        sim.open_window()
124
125    print("[INFO]: Stepping physics while ATTACHED (relative pose held):")
126    _run_phase(sim, cube_a, cube_b, attached=True)
127
128    # --- Phase 2: remove the constraint ------------------------------------
129    sim.remove_rigid_constraint("cube_weld")
130    assert "cube_weld" not in sim.get_rigid_constraint_uid_list()
131    print("\n[INFO]: Removed constraint 'cube_weld'. cube_a and cube_b are now free.")
132
133    import time
134
135    time.sleep(2.0)  # Wait a moment so the viewer can show the constraint removal.
136
137    print("[INFO]: Stepping physics while DETACHED (relative pose may drift):")
138    _run_phase(sim, cube_a, cube_b, attached=False)
139
140    print("\n[INFO]: Tutorial complete.")
141    sim.destroy()
142
143
144def _relative_z(cube_a, cube_b) -> float:
145    """Return the z-component of cube_b's pose relative to cube_a (env 0).
146
147    This reads the two bodies' world poses directly, so it works both while the
148    constraint is active (the value stays constant) and after removal (the
149    value drifts as the cubes move independently).
150
151    Args:
152        cube_a: The first :class:`RigidObject`.
153        cube_b: The second :class:`RigidObject`.
154
155    Returns:
156        The relative z (cube_b.z - cube_a.z) in meters.
157    """
158    pose_a = cube_a.get_local_pose(to_matrix=True)
159    pose_b = cube_b.get_local_pose(to_matrix=True)
160    return float(pose_b[0, 2, 3] - pose_a[0, 2, 3])
161
162
163def _run_phase(sim, cube_a, cube_b, attached: bool) -> None:
164    """Step the simulation for one phase and print the bodies' relative z.
165
166    Args:
167        sim: The :class:`SimulationManager`.
168        cube_a: The first :class:`RigidObject`.
169        cube_b: The second :class:`RigidObject`.
170        attached: True while the constraint is active, False after removal.
171    """
172    rel_z = _relative_z(cube_a, cube_b)
173    print(f"  step {0:4d}: relative z (cube_b - cube_a) = {rel_z:.4f} m")
174    for step in range(1, PHASE_STEPS + 1):
175        sim.update(step=STEPS_PER_UPDATE)
176        if step % PRINT_EVERY == 0:
177            rel_z = _relative_z(cube_a, cube_b)
178            print(f"  step {step:4d}: relative z (cube_b - cube_a) = {rel_z:.4f} m")
179
180
181if __name__ == "__main__":
182    main()

The Code Explained#

Adding two cubes#

Two dynamic cubes are added with SimulationManager.add_rigid_object(). Each uses a CubeCfg shape (a primitive cube, so no mesh asset file is needed) and a 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.

    cube_a = sim.add_rigid_object(
        cfg=RigidObjectCfg(
            uid="cube_a",
            shape=CubeCfg(size=[0.16, 0.16, 0.16]),
            attrs=physics_attrs,
            init_pos=[0.0, 0.0, 1.40],
        )
    )
    cube_b = sim.add_rigid_object(
        cfg=RigidObjectCfg(
            uid="cube_b",
            shape=CubeCfg(size=[0.16, 0.16, 0.16]),
            attrs=physics_attrs,
            init_pos=[0.0, 0.0, 1.20],
        )
    )

    if sim.is_use_gpu_physics:
        sim.init_gpu_physics()

    print("[INFO]: Scene setup complete with two cubes (cube_a, cube_b).")

Attaching the cubes#

The two cubes are welded with SimulationManager.create_rigid_constraint(). A 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.

    constraint = sim.create_rigid_constraint(
        cfg=RigidConstraintCfg(
            name="cube_weld",
            rigid_object_a_uid="cube_a",
            rigid_object_b_uid="cube_b",
        )
    )
    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 SimulationManager.remove_rigid_constraint(). After removal, 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.

    sim.remove_rigid_constraint("cube_weld")
    assert "cube_weld" not in sim.get_rigid_constraint_uid_list()
    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):

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:

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).