Construct a rigid body entity with different links/joints manually

[JonathanKuelz]

Thank you for providing genesis, it's been really fun to work with so far! While I like the examples and documentation provided so far, I didn't find an answer to this supposedly easy problem:

Problem Description: How can i create a rigid body entity with multiple degrees of freedom manually?

Background: Not every robot / rigid body mechanism that is interesting for simulation is available as urdf / xml file. Also, it would be beneficial if runtime-manipulation of object properties, e.g. attaching different objects to a joint, would be possible.

Example: I tried to create a model of a simple carabiner - the main part is fixed to the world origin, whereas the moving part (the gate) should be connected to the main body via a 1 DOF revolute joint. For the moment, let's ignore friction and any kind of spring forces. I have mesh files for both parts of the carabiner (arbitrary file format).

I did not find any documentation or example on how to set up such a model. Instead, I looked at how URDFs are parsed and tried to replicate this workflow for a simple example. I added the main body as entity to the scene and then went on to add the second link and a joint. While the initial scene looks just fine, as first as I take the first simulation body, the second body (the gate) disappears and I can't figure out why.

So here's my question: Is it possible to manually set up multiple rigid bodies, parsed from different mesh files, connect them with joints, and simulate them? What should I have done differently? Any help resolving this issue would be greatly appreciated. Let me know if you are developing this functionality and how I can contribute.

Additional Info: At first, i though the problem might be that the two bodies are initially in collision (I am not sure if they are). However, the same issue arises (gate disappears) if I add an offset so the two definitely don't collide.

Here's the code for my example. Happy to send the mesh files if necessary, I just extracted them from the CAD file available here.

from pathlib import Path
import sys

import numpy as np
from omegaconf import DictConfig

import genesis as gs
import genesis.utils.geom as gu
import hydra


CARABINER_MAIN = 'mine/carabiner/Carabiner_main.gltf'
CARABINER_GATE = 'mine/carabiner/Carabiner_gate.gltf'
pos = np.array([4.62, 5.75, 0]) / 100

def get_joint_data(**kwargs):
    joint_data = dict(
        name='jnt',
        n_qs=1,
        n_dofs=1,
        type=gs.JOINT_TYPE.REVOLUTE,
        pos=pos,
        quat=(1, 0, 0, 0),
        dofs_motion_ang=np.zeros((0, 3)),
        dofs_motion_vel=np.zeros((0, 3)),
        dofs_limit=np.array(((-45, 0),)),  # TODO
        dofs_invweight=gu.default_dofs_invweight(1),
        dofs_stiffness=np.array(((1.0,),)),  # TODO
        dofs_sol_params=gu.default_solver_params(1),
        dofs_damping=gu.default_dofs_damping(1),
        dofs_armature=gu.default_dofs_armature(1),
        dofs_kp=gu.default_dofs_kp(1),
        dofs_kv=gu.default_dofs_kv(1),
        dofs_force_range=gu.default_dofs_force_range(1),
        init_qpos=(0,),
    )
    joint_data.update(**kwargs)
    return joint_data

def add_carabiner(scene: gs.Scene):
    """Adds the pieces of a carabiner to the scene and connects them."""

    carabiner = scene.add_entity(
        gs.morphs.Mesh(file=CARABINER_MAIN, fixed=True, scale=10)
    )
    carabiner._add_joint(**get_joint_data())
    gate = carabiner._add_link(
        name='gate',
        pos=pos,
        quat=(1., 0., 0., 0.),
        inertial_pos=gu.zero_pos(),
        inertial_quat=gu.identity_quat(),
        inertial_i=None,
        inertial_mass=None,
        parent_idx=0,
        invweight=-1
    )
    morph = gs.morphs.Mesh(file=CARABINER_GATE, scale=10)
    meshes = gs.Mesh.from_morph_surface(morph, carabiner.surface)
    for mesh in meshes:
        gate._add_geom(
            mesh=mesh,
            init_pos=gu.zero_pos(),
            init_quat=gu.identity_quat(),
            friction=carabiner.material.friction,
            data=None,
            needs_coup=carabiner.material.needs_coup,
            type=gs.GEOM_TYPE.MESH,
            sol_params=gu.default_solver_params(n=1)[0]
        )
        gate._add_vgeom(
            vmesh=mesh,
            init_pos=gu.zero_pos(),
            init_quat=gu.identity_quat(),
        )

    return carabiner

@hydra.main(config_path="cfg", config_name='config', version_base="1.2")
def main():
    gs.init()

    scene = gs.Scene(show_viewer=True)
    carabiner = add_carabiner(scene)

    scene.build()

    q_start = 0
    q_end = -1
    for i in range(cfg.steps):
        q = q_start + q_end * i / (cfg.steps - 1)
        q = q * np.pi / 180
        carabiner.set_dofs_position(np.array([q]))  # Removing this still results in a broken simulation
        scene.step()
    input("Press Enter to exit...")



if __name__ == '__main__':
    main()

[JonathanKuelz]

Screenshot for context: This is the carabiner, before and after the first scene.step() .

[Kashu7100]

Not completely sure but you might want to try scene.link_entities() (Example: https://github.com/Genesis-Embodied-AI/Genesis/blob/main/examples/rigid/merge_entities.py) for combining different rigid entities.

[JonathanKuelz]

Thanks for pointing me toward link_entities, it looks interesting, but I think I am looking for a lower-level API. link_entitiesseems to work on "finished" entities, I am looking for a possibility to assemble multiple rigid bodies in a kinematic chain - and it would be important for me to be able to control the connecting joints between these bodies,