[BUG] Non-root bodies of kinematic chain do not collide with floor

[JonathanKuelz]

Bug Description

Hello warp team, I'm not sure if reporting a Bug or an undocumented feature here, but at least I believe this is not how you would intuitively expect things to work:

I wanted to understand collisions and the nececity for the eval_fk function better in the presence of joints / kinematic chains, so I created a simple script that builds a "walker" made out of

• A sphere "body"
• Two capsules ("legs") attached to this sphere vie revolute joints

The created "walker" does not behave as expected. When I set it in a scene, a bit above the ground, it falls down, but only the root body (the sphere), not the "legs" collide with the floor. I couldn't find the reason, instead:

• The "legs" collide with other obstacles in the environment
• If I create the same primitive, not attached to another body via a joint, it does collide with the floor
• self.model.rigid_contact_count seems to count the collisions
• Setting has_ground_collision=True explicitly doesn't change anything

Here are some screenshots to illustrate the reported behavior:

1. The "walker"
   

2. No collision between floor and legs
   

3. Collision between body and floor
   

4. Collision with other primitive / other primitive does not fall through the floor
   

Here's the code to reproduce:

from pathlib import Path
from typing import List

import numpy as np
import warp as wp
import warp.optim
import warp.sim  # Necessary for the model builder, even though not explicitly used here
import warp.sim.render



class Demo:

    def __init__(self,
                 stage_path: str | Path = "joint_demo.usda",
                 sim_substeps: int = 5,
                 num_frames: int = 240,
                 fps: int = 60,
                 render: bool = True,
                 ):
        builder = wp.sim.ModelBuilder()

        walker = builder.add_body(
            origin=wp.transform((0., 1.5, 0.), wp.quat_from_axis_angle(wp.vec3(0., 1., 0.), -np.pi / 4)),
            name='walker')
        c = builder.add_shape_sphere(
            body=walker,
            radius=.25,
        )
        left_leg = builder.add_body(origin=wp.transform((0., 0., 0.), wp.quat_identity()), name='left_leg')
        left = builder.add_shape_capsule(
            body=left_leg,
            pos=wp.vec3(0, 0., 0.),  # relative to walker
            rot=wp.quat_identity(),
            radius=.1,
            half_height=.75 / 2,
            has_ground_collision=True
        )
        right_leg = builder.add_body(origin=wp.transform((0., 0., 0.), wp.quat_identity()), name='right_leg')
        right = builder.add_shape_capsule(
            body=right_leg,
            pos=wp.vec3(0, 0., 0.),  # relative to walker
            rot=wp.quat_identity(),
            radius=.1,
            half_height=.75 / 2,
            has_ground_collision=True
        )
        builder.add_joint_revolute(
            parent=walker,
            child=left_leg,
            parent_xform=wp.transform((0., 0, .375), wp.quat_identity()),
            child_xform=wp.transform((0., 0.15, 0.), wp.quat_identity()),
            axis=wp.vec3(0., 0., 1.),
        )
        builder.add_joint_revolute(
            parent=walker,
            child=right_leg,
            parent_xform=wp.transform((0., 0., -.375), wp.quat_identity()),
            child_xform=wp.transform((0., 0.15, 0.), wp.quat_identity()),
            axis=wp.vec3(0., 0., 1.),
        )
        cc = builder.add_body(
            # origin=wp.transform((0., 1.5, 1.), wp.quat_rpy(np.pi / 3, 0., 0.)),
            origin=wp.transform((0.4, .1, .3), wp.quat_rpy(np.pi / 2, 0., 0.)),
            name='collision_capsule'
        )
        builder.add_shape_capsule(
            body=cc,
            radius=.1,
            half_height=.75 / 2,
        )
        builder.joint_q = np.array([-np.pi / 8, np.pi / 8])

        self.frame_dt = 1.0 / fps
        self.num_frames = num_frames
        self.frame: int = 0
        self.sim_substeps: int = sim_substeps
        self.sim_dt = self.frame_dt / self.sim_substeps

        self.sim_time: float = 0.
        self.model = builder.finalize()
        self.model.ground = True

        self.integrator = wp.sim.SemiImplicitIntegrator()
        # self.integrator = wp.sim.FeatherstoneIntegrator(self.model)

        if render:
            self.renderer = wp.sim.render.SimRenderer(self.model, stage_path, scaling=1.0)

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

        wp.sim.eval_fk(self.model, self.model.joint_q, self.model.joint_qd, None, self.state_0)

    def simulate(self):
        for s in range(self.sim_substeps):
            self.state_0.clear_forces()
            wp.sim.eval_fk(self.model, self.model.joint_q, self.model.joint_qd, None, self.state_0)
            wp.sim.collide(self.model, self.state_0)
            self.integrator.simulate(self.model, self.state_0, self.state_1, self.sim_dt)
            self.state_0, self.state_1 = self.state_1, self.state_0

    def forward(self):
        for step in range(self.num_frames):
            self.simulate()
            print(f"frame {step}, rigid contact count {self.model.rigid_contact_count}")
            self.sim_time += self.frame_dt
            if self.renderer:
                self.render(self.state_0)

        if self.renderer:
            self.renderer.save()

    def render(self, state):
        self.renderer.begin_frame(self.sim_time)
        self.renderer.render(state)
        self.renderer.end_frame()


if __name__ == '__main__':
    wp.init()

    sim = Demo()
    sim.forward()

System Information

No response