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

Contents