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

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 RigidBodyPhysicsCfg 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],
        )
    )

    sim.prepare()

    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