From 8cacfaf7d96144d5a66a2fb943914ee036645913 Mon Sep 17 00:00:00 2001 From: chenjian Date: Tue, 31 Mar 2026 12:19:45 +0800 Subject: [PATCH 1/4] update --- docs/source/overview/sim/sim_articulation.md | 4 +- docs/source/overview/sim/sim_assets.md | 1 + docs/source/overview/sim/sim_cloth.md | 168 +++++++++++++++++++ docs/source/overview/sim/sim_soft_object.md | 2 +- docs/source/tutorial/create_cloth.rst | 3 +- embodichain/lab/sim/objects/articulation.py | 2 +- embodichain/lab/sim/objects/robot.py | 35 ++++ 7 files changed, 209 insertions(+), 6 deletions(-) create mode 100644 docs/source/overview/sim/sim_cloth.md diff --git a/docs/source/overview/sim/sim_articulation.md b/docs/source/overview/sim/sim_articulation.md index 51d5dca8..ecbc518d 100644 --- a/docs/source/overview/sim/sim_articulation.md +++ b/docs/source/overview/sim/sim_articulation.md @@ -24,7 +24,7 @@ Articulations are configured using the {class}`~cfg.ArticulationCfg` dataclass. ### Drive Configuration -The `drive_props` parameter controls the joint physics behavior. It is defined using the `JointDrivePropertiesCfg` class. +The `drive_props` parameter controls the joint physics behavior. It is defined using the `JointDrivePropertiesCfg` class. For articulation object without internal drive force, like cabinet and drawer, better set `drive_type` to `"none"`. | Parameter | Type | Default | Description | | :--- | :--- | :--- | :--- | @@ -33,7 +33,7 @@ The `drive_props` parameter controls the joint physics behavior. It is defined u | `max_effort` | `float` / `Dict` | `1.0e10` | Maximum effort (force/torque) the joint can exert. | | `max_velocity` | `float` / `Dict` | `1.0e10` | Maximum velocity allowed for the joint ($m/s$ or $rad/s$). | | `friction` | `float` / `Dict` | `0.0` | Joint friction coefficient. | -| `drive_type` | `str` | `"force"` | Drive mode: `"force"`(driven by a force), `"acceleration"`(driven by an acceleration) or `none`(no force). | +| `drive_type` | `str` | `"none"` | Drive mode: `"force"`(driven by a force), `"acceleration"`(driven by an acceleration) or `none`(no force). | ### Setup & Initialization diff --git a/docs/source/overview/sim/sim_assets.md b/docs/source/overview/sim/sim_assets.md index 149e3e35..ab49dfbb 100644 --- a/docs/source/overview/sim/sim_assets.md +++ b/docs/source/overview/sim/sim_assets.md @@ -127,6 +127,7 @@ Configured via `LightCfg`. sim_rigid_object.md sim_rigid_object_group.md +sim_cloth.md sim_soft_object.md sim_articulation.md sim_robot.md diff --git a/docs/source/overview/sim/sim_cloth.md b/docs/source/overview/sim/sim_cloth.md new file mode 100644 index 00000000..78cc4bf5 --- /dev/null +++ b/docs/source/overview/sim/sim_cloth.md @@ -0,0 +1,168 @@ +# Cloth Object + +```{currentmodule} embodichain.lab.sim +``` + +The {class}`~objects.Cloth` class represents deformable surface entities in EmbodiChain. Unlike rigid bodies, cloth objects are defined by vertices and meshes rather than a single rigid pose. + +## Configuration + +Configured via {class}`~cfg.ClothObjectCfg`. + +| Parameter | Type | Default | Description | +| :--- | :--- | :--- | :--- | +| `physical_attr` | `ClothPhysicalAttributesCfg` | `...` | Physical attributes. | +| `shape` | `MeshCfg` | `MeshCfg()` | Mesh configuration. | + +### CLoth Body Attributes + +Cloth bodies require both voxelization and physical attributes. + +**Physical Attributes ({class}`~cfg.ClothPhysicalAttributesCfg`)** + +| Parameter | Type | Default | Description | +| :--- | :--- | :--- | :--- | +| `youngs` | `float` | `1e10` | Young's modulus (higher = stiffer). | +| `poissons` | `float` | `0.3` | Poisson's ratio. | +| `dynamic_friction` | `float` | `0.5` | Dynamic friction coefficient. | +| `elasticity_damping` | `float` | `0.0` | Elasticity damping factor. | +| `thickness` | `float` | `0.01` | Cloth thickness (m). | +| `bending_stiffness` | `float` | `0.001` | Bending stiffness. | +| `bending_damping` | `float` | `0.0` | Bending damping. | +| `enable_kinematic` | `bool` | `False` | If True, (partially) kinematic behavior is enabled. | +| `enable_ccd` | `bool` | `True` | Enable continuous collision detection (CCD). | +| `enable_self_collision` | `bool` | `False` | Enable self-collision handling. | +| `has_gravity` | `bool` | `True` | Whether the cloth is affected by gravity. | +| `self_collision_stress_tolerance` | `float` | `0.9` | Stress tolerance threshold for self-collision constraints. | +| `collision_mesh_simplification` | `bool` | `True` | Whether to simplify the collision mesh for self-collision. | +| `vertex_velocity_damping` | `float` | `0.005` | Per-vertex velocity damping. | +| `mass` | `float` | `-1.0` | Total mass of the cloth. If negative, density is used to compute mass. | +| `density` | `float` | `1.0` | Material density in kg/m^3. | +| `max_depenetration_velocity` | `float` | `1e6` | Maximum velocity used to resolve penetrations. | +| `max_velocity` | `float` | `100.0` | Clamp for linear (or vertex) velocity. | +| `self_collision_filter_distance` | `float` | `0.1` | Distance threshold for filtering self-collision vertex pairs. | +| `linear_damping` | `float` | `0.05` | Global linear damping applied to the cloth. | +| `sleep_threshold` | `float` | `0.05` | Velocity/energy threshold below which the cloth can go to sleep. | +| `settling_threshold` | `float` | `0.1` | Threshold used to decide convergence/settling state. | +| `settling_damping` | `float` | `10.0` | Additional damping applied during settling phase. | +| `min_position_iters` | `int` | `4` | Minimum solver iterations for position correction. | +| `min_velocity_iters` | `int` | `1` | Minimum solver iterations for velocity updates. | + +For Cloth Object tutorial, please refer to the [Cloth Body Simulation](https://dexforce.github.io/EmbodiChain/tutorial/create_cloth.html). + + +### Setup & Initialization + +```python +import torch +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.objects import ClothObject, ClothObjectCfg + + +def create_2d_grid_mesh(width: float, height: float, nx: int = 1, ny: int = 1): + """Create a flat rectangle in the XY plane centered at `origin`. + + The rectangle is subdivided into an `nx` by `ny` grid (cells) and + triangulated. `nx=1, ny=1` yields the simple two-triangle rectangle. + + Returns an vertices and triangles. + """ + w = float(width) + h = float(height) + if nx < 1 or ny < 1: + raise ValueError("nx and ny must be >= 1") + + # Vectorized vertex positions using PyTorch + x_lin = torch.linspace(-w / 2.0, w / 2.0, steps=nx + 1, dtype=torch.float64) + y_lin = torch.linspace(-h / 2.0, h / 2.0, steps=ny + 1, dtype=torch.float64) + yy, xx = torch.meshgrid(y_lin, x_lin) # shapes: (ny+1, nx+1) + xx_flat = xx.reshape(-1) + yy_flat = yy.reshape(-1) + zz_flat = torch.full_like(xx_flat, 0, dtype=torch.float64) + verts = torch.stack([xx_flat, yy_flat, zz_flat], dim=1) # (Nverts, 3) + + # Vectorized triangle indices + idx = torch.arange((nx + 1) * (ny + 1), dtype=torch.int64).reshape(ny + 1, nx + 1) + v0 = idx[:-1, :-1].reshape(-1) + v1 = idx[:-1, 1:].reshape(-1) + v2 = idx[1:, :-1].reshape(-1) + v3 = idx[1:, 1:].reshape(-1) + tri1 = torch.stack([v0, v1, v3], dim=1) + tri2 = torch.stack([v0, v3, v2], dim=1) + faces = torch.cat([tri1, tri2], dim=0).to(torch.int32) + return verts, faces + +# 1. Initialize Simulation +device = "cuda" if torch.cuda.is_available() else "cpu" +sim_cfg = SimulationManagerCfg(sim_device=device) +sim = SimulationManager(sim_config=sim_cfg) + +cloth_verts, cloth_faces = create_2d_grid_mesh(width=0.3, height=0.3, nx=12, ny=12) +cloth_mesh = o3d.geometry.TriangleMesh( + vertices=o3d.utility.Vector3dVector(cloth_verts.to("cpu").numpy()), + triangles=o3d.utility.Vector3iVector(cloth_faces.to("cpu").numpy()), +) +cloth_save_path = os.path.join(tempfile.gettempdir(), "cloth_mesh.ply") +o3d.io.write_triangle_mesh(cloth_save_path, cloth_mesh) +# 2. Configure Cloth Object +cfg=ClothObjectCfg( + uid="cloth_demo", + shape=MeshCfg(fpath=cloth_save_path), + init_pos=[0.5, 0.0, 0.3], + init_rot=[0, 0, 0], + physical_attr=ClothPhysicalAttributesCfg( + mass=0.01, + youngs=1e10, + poissons=0.4, + thickness=0.04, + bending_stiffness=0.01, + bending_damping=0.1, + dynamic_friction=0.95, + min_position_iters=30, + ), +) + +# 3. Spawn Cloth Object +# Note: Assuming the method in SimulationManager is 'add_cloth_object' +cloth_object: ClothObject = sim.add_cloth_object(cfg=cfg) + +# 4. Initialize Physics +sim.reset_objects_state() +``` +### Cloth Object Class +#### Vertex Data (Observation) +For cloth objects, the state is represented by the positions and velocities of its vertices, rather than a single root pose. + +| Method | Return Shape | Description | +| :--- | :--- | :--- | +| `get_current_vertex_position()` | `(n_envs, n_vert, 3)` | Current positions of mesh vertices. | +| `get_current_vertex_velocity()` | `(n_envs, n_vert, 3)` | Current positions of mesh vertices. | +| `get_rest_vertex_position()` | `(n_envs, n_vert, 3` | Rest (initial) positions of collision vertices. | + +> Note: N is the number of environments/instances, V_col is the number of collision vertices, and V_sim is the number of simulation vertices. + +```python +# Example: Accessing vertex data +vert_position = cloth_object.get_current_vertex_position() +print(f"vertices positions: {vert_position}") + +vert_velocity = cloth_object.get_current_vertex_velocity() +print(f"Vertex Velocities: {vert_velocity}") +``` +#### Pose Management +You can set the global pose of a cloth object (which transforms all its vertices), but getting a single "pose" from a deformed surface object is not supported. + +| Method | Description | +| :--- | :--- | +| `set_local_pose(pose)` | Sets the pose of the object by transforming all vertices. | +| `get_local_pose()` | **Not Supported**. Raises `NotImplementedError` because a deformed object does not have a single rigid pose. | + + +```python +# Reset or Move the Cloth Object +target_pose = torch.tensor([[0, 0, 1.0, 1, 0, 0, 0]], device=device) # (x, y, z, qw, qx, qy, qz) +cloth_object.set_local_pose(target_pose) + +# Important: Step simulation to apply changes +sim.update() +``` \ No newline at end of file diff --git a/docs/source/overview/sim/sim_soft_object.md b/docs/source/overview/sim/sim_soft_object.md index 4f8b0101..d8b9f510 100644 --- a/docs/source/overview/sim/sim_soft_object.md +++ b/docs/source/overview/sim/sim_soft_object.md @@ -3,7 +3,7 @@ ```{currentmodule} embodichain.lab.sim ``` -The {class}`~objects.SoftObject` class represents deformable entities (e.g., cloth, sponges, soft robotics) in EmbodiChain. Unlike rigid bodies, soft objects are defined by vertices and meshes rather than a single rigid pose. +The {class}`~objects.SoftObject` class represents deformable entities (e.g. sponges, soft robotics) in EmbodiChain. Unlike rigid bodies, soft objects are defined by vertices and meshes rather than a single rigid pose. ## Configuration diff --git a/docs/source/tutorial/create_cloth.rst b/docs/source/tutorial/create_cloth.rst index c43bc091..940d5178 100644 --- a/docs/source/tutorial/create_cloth.rst +++ b/docs/source/tutorial/create_cloth.rst @@ -63,8 +63,7 @@ The grid mesh generated earlier is saved to disk and then passed to :meth:`Simul .. literalinclude:: ../../../scripts/tutorials/sim/create_cloth.py :language: python :start-at: cloth_verts, cloth_faces = create_2d_grid_mesh - :end-at: ) - :end-before: padding_box_cfg + :end-at: padding_box_cfg = RigidObjectCfg Adding a rigid body for interaction ------------------------------------- diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index 10dbebec..4ecfd1bf 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -1235,7 +1235,7 @@ def set_joint_drive( max_effort: torch.Tensor | None = None, max_velocity: torch.Tensor | None = None, friction: torch.Tensor | None = None, - drive_type: str = "force", + drive_type: str = "none", joint_ids: Sequence[int] | None = None, env_ids: Sequence[int] | None = None, ) -> None: diff --git a/embodichain/lab/sim/objects/robot.py b/embodichain/lab/sim/objects/robot.py index c03761c5..fc38dae6 100644 --- a/embodichain/lab/sim/objects/robot.py +++ b/embodichain/lab/sim/objects/robot.py @@ -792,6 +792,41 @@ def _init_control_parts(self, control_parts: Dict[str, List[str]]) -> None: # Initialize control groups self._control_groups = self._extract_control_groups() + def set_joint_drive( + self, + stiffness: torch.Tensor | None = None, + damping: torch.Tensor | None = None, + max_effort: torch.Tensor | None = None, + max_velocity: torch.Tensor | None = None, + friction: torch.Tensor | None = None, + drive_type: str = "force", + joint_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | None = None, + ) -> None: + """Set the drive properties for the robot. + Different from Articulation, default drive type is 'force' instead of 'none' + + Args: + stiffness (torch.Tensor): The stiffness of the joint drive with shape (len(env_ids), len(joint_ids)). + damping (torch.Tensor): The damping of the joint drive with shape (len(env_ids), len(joint_ids)). + max_effort (torch.Tensor): The maximum effort of the joint drive with shape (len(env_ids), len(joint_ids)). + max_velocity (torch.Tensor): The maximum velocity of the joint drive with shape (len(env_ids), len(joint_ids)). + friction (torch.Tensor): The joint friction coefficient with shape (len(env_ids), len(joint_ids)). + drive_type (str, optional): The type of drive to apply. Defaults to "force". + joint_ids (Sequence[int] | None, optional): The joint indices to apply the drive to. If None, applies to all joints. Defaults to None. + env_ids (Sequence[int] | None, optional): The environment indices to apply the drive to. If None, applies to all environments. Defaults to None. + """ + super().set_joint_drive( + stiffness=stiffness, + damping=damping, + max_effort=max_effort, + max_velocity=max_velocity, + friction=friction, + drive_type=drive_type, + joint_ids=joint_ids, + env_ids=env_ids, + ) + def _set_default_joint_drive(self) -> None: """Set default joint drive parameters based on the configuration.""" import numbers From 506fdfece52a313f0d6eabf2912ae4dbc62fef45 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 31 Mar 2026 15:16:27 +0800 Subject: [PATCH 2/4] wip --- embodichain/lab/sim/cfg.py | 2 +- examples/sim/demo/pick_up_cloth.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/embodichain/lab/sim/cfg.py b/embodichain/lab/sim/cfg.py index 5063e5ce..36ca926f 100644 --- a/embodichain/lab/sim/cfg.py +++ b/embodichain/lab/sim/cfg.py @@ -1114,7 +1114,7 @@ class ArticulationCfg(ObjectBaseCfg): fpath: str = None """Path to the articulation asset file.""" - drive_pros: JointDrivePropertiesCfg = JointDrivePropertiesCfg() + drive_pros: JointDrivePropertiesCfg = JointDrivePropertiesCfg(drive_type="none") """Properties to define the drive mechanism of a joint.""" body_scale: Union[tuple, list] = (1.0, 1.0, 1.0) diff --git a/examples/sim/demo/pick_up_cloth.py b/examples/sim/demo/pick_up_cloth.py index ecc57188..36d1c243 100644 --- a/examples/sim/demo/pick_up_cloth.py +++ b/examples/sim/demo/pick_up_cloth.py @@ -63,7 +63,7 @@ def parse_arguments(): "--enable_rt", action="store_true", help="Enable ray tracing rendering" ) parser.add_argument( - "--num_envs", type=int, default=4, help="Number of parallel environments" + "--num_envs", type=int, default=1, help="Number of parallel environments" ) return parser.parse_args() From 5b16c05a3e99c63f44b5263dbf539d83d774413a Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 31 Mar 2026 15:44:23 +0800 Subject: [PATCH 3/4] wip --- tests/sim/objects/test_articulation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/sim/objects/test_articulation.py b/tests/sim/objects/test_articulation.py index bb4dc735..8140b775 100644 --- a/tests/sim/objects/test_articulation.py +++ b/tests/sim/objects/test_articulation.py @@ -44,7 +44,7 @@ def setup_simulation(self, sim_device): art_path = get_data_path(ART_PATH) assert os.path.isfile(art_path) - cfg_dict = {"fpath": art_path} + cfg_dict = {"fpath": art_path, "drive_pros": {"drive_type": "force"}} self.art: Articulation = self.sim.add_articulation( cfg=ArticulationCfg.from_dict(cfg_dict) ) From e389d1bc37f29f9ad706311e0ee7126332220bbd Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 31 Mar 2026 18:40:31 +0800 Subject: [PATCH 4/4] wip --- embodichain/lab/sim/cfg.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/embodichain/lab/sim/cfg.py b/embodichain/lab/sim/cfg.py index 36ca926f..5e8e1f22 100644 --- a/embodichain/lab/sim/cfg.py +++ b/embodichain/lab/sim/cfg.py @@ -1174,6 +1174,9 @@ class RobotCfg(ArticulationCfg): """Configuration for a robot asset in the simulation. """ + drive_pros: JointDrivePropertiesCfg = JointDrivePropertiesCfg(drive_type="force") + """Properties to define the drive mechanism of a joint.""" + control_parts: Dict[str, List[str]] | None = None """Control parts is the mapping from part name to joint names.