[QUESTION] Collision between Soft Bodies

[XJay18]

Description

When I add two soft grids to the scene and then do a free-fall simulation, the two soft grids penetrate each other. Am I missing anything in the simulation? Any comments are welcome! Thanks in advance!

Code

import math
import os
from enum import Enum

import numpy as np

import warp as wp
import warp.sim
import warp.sim.render

import imageio
import matplotlib.pyplot as plt


class IntegratorType(Enum):
    EULER = "euler"
    XPBD = "xpbd"

    def __str__(self):
        return self.value


class Example:
    def __init__(
        self, integrator: IntegratorType = IntegratorType.EULER
    ):
        self.integrator_type = integrator

        fps = 30
        self.sim_substeps = 256
        self.frame_dt = 1.0 / fps
        self.sim_dt = self.frame_dt / self.sim_substeps
        print(self.sim_dt)
        self.sim_time = 0.0
        self.profiler = {}

        builder = wp.sim.ModelBuilder()
        builder.default_particle_radius = 0.01  # Contact radius

        youngs = 1e5
        poissons = 0.3
        k_mu = youngs / (2.0 * (1.0 + poissons))
        k_lambda = (youngs * poissons) / ((1.0 + poissons) * (1.0 - 2.0 * poissons))

        particle_density = 100.0
        print(f'k_mu: {k_mu}, k_lambda: {k_lambda}, rho: {particle_density}')

        builder.add_soft_grid(
            pos=wp.vec3(-1.0, 1.5, 0.0),
            rot=wp.quat_identity(),
            vel=wp.vec3(0.0, 0.0, 0.0),
            dim_x=20,
            dim_y=10,
            dim_z=10,
            cell_x=0.1,
            cell_y=0.1,
            cell_z=0.1,
            density=particle_density,
            k_mu=k_mu,
            k_lambda=k_lambda,
            k_damp=0.01,
        )

        builder.add_soft_grid(
            pos=wp.vec3(0.5, 3, 0.0),
            rot=wp.quat_from_axis_angle(wp.vec3(0.0, 1.0, 0.0), -math.pi / 2),
            vel=wp.vec3(0.0, 0.0, 0.0),
            dim_x=20,
            dim_y=10,
            dim_z=10,
            cell_x=0.1,
            cell_y=0.1,
            cell_z=0.1,
            density=particle_density,
            k_mu=k_mu,
            k_lambda=k_lambda,
            k_damp=0.01,
        )

        builder.set_ground_plane()

        self.model = builder.finalize()
        self.model.ground = True

        self.model.soft_contact_ke = 2.0e3
        self.model.soft_contact_kd = 0.1
        self.model.soft_contact_kf = 10.0
        self.model.soft_contact_mu = 0.7
        self.model.soft_contact_restitution = 0.0

        if self.integrator_type == IntegratorType.EULER:
            self.integrator = wp.sim.SemiImplicitIntegrator()
        elif self.integrator_type == IntegratorType.XPBD:
            self.integrator = wp.sim.XPBDIntegrator(iterations=1)
        else:
            raise ValueError(f"Unknown integrator type: {self.integrator_type}")

        self.state_0 = self.model.state()
        self.state_1 = self.model.state()

        self.renderer = wp.sim.render.SimRendererOpenGL(
            self.model,
            'output_frames',
            scaling=1.0,
            camera_pos=(7.0, 3.0, 7.0),
            camera_front=(-1.0, 0., -1.0),
            camera_up=(0.0, 1.0, 0.0),
            headless=True
        )
        # Create pixel buffer with correct shape
        channels = 3  # RGB
        pixel_shape = (self.renderer.screen_height, self.renderer.screen_width, channels)

        self.pixels = wp.zeros(pixel_shape, dtype=wp.float32)
        self.frame_num = 0

        self.use_cuda_graph = wp.get_device().is_cuda
        if self.use_cuda_graph:
            with wp.ScopedCapture() as capture:
                self.simulate()
            self.graph = capture.graph

    def simulate(self):
        for _ in range(self.sim_substeps):
            wp.sim.collide(self.model, self.state_0)

            self.state_0.clear_forces()
            self.state_1.clear_forces()

            self.integrator.simulate(self.model, self.state_0, self.state_1, self.sim_dt)

            # swap states
            (self.state_0, self.state_1) = (self.state_1, self.state_0)

    def step(self):
        with wp.ScopedTimer("step", print=False, dict=self.profiler):
            if self.use_cuda_graph:
                wp.capture_launch(self.graph)
            else:
                self.simulate()
            self.sim_time += self.frame_dt

    def render(self, video_writer=None, image_dir=None):
        if self.renderer is None:
            return

        with wp.ScopedTimer("render", print=False):
            self.renderer.begin_frame(self.sim_time)
            self.renderer.render(self.state_0)

            # Get pixels from renderer
            self.renderer.get_pixels(
                self.pixels, 
                split_up_tiles=False,  # Set to True if using tiled rendering
                mode="rgb",
            )

            # Get pixels from renderer
            self.renderer.get_pixels(
                self.pixels, 
                split_up_tiles=False,  # Set to True if using tiled rendering
                mode="rgb",
            )

            self.frame_num += 1
            self.renderer.end_frame()

        # Save single frame
        frame = self.pixels.numpy()
        if video_writer is not None:
            # Convert to uint8
            frame = (frame * 255).astype(np.uint8)
            video_writer.append_data(frame)
        
        if image_dir is not None:
            output_path = os.path.join(image_dir, f"frame_{self.frame_num:04d}.png")
            plt.imsave(output_path, frame)


if __name__ == "__main__":
    import argparse

    parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
    parser.add_argument("--device", type=str, default=None, help="Override the default Warp device.")
    parser.add_argument("--video_path", type=str, default=None, help="Path to the video file.")
    parser.add_argument("--image_path", type=str, default=None, help="Path to the rendered output.")
    parser.add_argument("--num_frames", type=int, default=300, help="Total number of frames.")
    parser.add_argument(
        "--integrator",
        help="Type of integrator",
        type=IntegratorType,
        choices=list(IntegratorType),
        default=IntegratorType.EULER,
    )

    args = parser.parse_known_args()[0]

    with wp.ScopedDevice(args.device):
        example = Example(integrator=args.integrator)

        output_dir = None
        video_writer = None

        if args.video_path is not None:
            video_writer = imageio.get_writer(args.video_path, fps=60)
        
        if args.image_path is not None:
             # Save pixels to file
            output_dir = args.image_path
            os.makedirs(output_dir, exist_ok=True)

        for _i in range(args.num_frames):
            example.step()
            example.render(video_writer, output_dir)

        frame_times = example.profiler["step"]
        print("\nAverage frame sim time: {:.2f} ms".format(sum(frame_times) / len(frame_times)))

        if args.video_path is not None:
            video_writer.close()

        if example.renderer:
            example.renderer.clear()

Result