[FEATURE] Allow mounted cameras to be run in parallel simulations

[abhijitmajumdar]

Description

When running in parallel environments, while attaching the camera to the link, you need to specify the environment index (env_idx) for each camera. This offsets the local Transform (T) to the corresponding environment offset

n_envs = 4
cams = [scene.add_camera(GUI=True, fov=70) for _ in range(n_envs)]
for nenv, cam in enumerate(cams):
        cam.attach(franka.get_link("hand"), np.eye(4), env_idx=nenv)

Related Issue

Resolves #1016

Motivation and Context

Previous PR introduced mounting cameras to the rigid body links (example: robot end-effector). However it did not support running such mounted cameras in parallel environments.

How Has This Been / Can This Be Tested?

To run in parallel environments, use this (change the n_envs to the number of parallel simulations to run)

import genesis as gs
import numpy as np

def main():
    
    gs.init()
    
    scene = gs.Scene(show_viewer=True)
    scene.add_entity(gs.morphs.Plane())
    scene.add_entity(gs.morphs.Box(pos=(0.5, 0.0, 0.0), size=(0.05, 0.05, 0.05)))
    franka = scene.add_entity(gs.morphs.MJCF(file='xml/franka_emika_panda/panda.xml'))
    
    n_envs = 4
    env_spacing = (2.0, 2.0)
    cams = [scene.add_camera(GUI=True, fov=70) for _ in range(n_envs)]
    scene.build(n_envs=n_envs, env_spacing=env_spacing)

    T = np.eye(4)
    T[:3, :3] = np.array([
        [1,  0,  0],
        [0, -1,  0],
        [0,  0, -1]
    ])
    T[:3, 3] = np.array([0.1, 0.0, 0.1])
    for nenv, cam in enumerate(cams):
        cam.attach(franka.get_link("hand"), T, nenv)

    target_quat = np.tile(np.array([0, 1, 0, 0]), [n_envs, 1]) # pointing downwards
    center = np.tile(np.array([0.5, 0.0, 0.5]), [n_envs, 1])
    angular_speed = np.random.uniform(-10, 10, n_envs)
    r = 0.1

    ee_link = franka.get_link('hand')

    for i in range(1000):
        target_pos = np.zeros([n_envs, 3])
        target_pos[:, 0] = center[:, 0] + np.cos(i/360*np.pi*angular_speed) * r
        target_pos[:, 1] = center[:, 1] + np.sin(i/360*np.pi*angular_speed) * r
        target_pos[:, 2] = center[:, 2]

        q = franka.inverse_kinematics(
            link     = ee_link,
            pos      = target_pos,
            quat     = target_quat,
            rot_mask = [False, False, True], # for demo purpose: only restrict direction of z-axis
        )

        franka.set_qpos(q)
        scene.step()
        for cam in cams:
            cam.render()
        
if __name__ == "__main__":
    main()

To verify this is still functional on non parallel environments, run this

import genesis as gs
import numpy as np

def main():
    
    gs.init()
    
    scene = gs.Scene(show_viewer=True)
    scene.add_entity(gs.morphs.Plane())
    scene.add_entity(gs.morphs.Box(pos=(0.5, 0.0, 0.0), size=(0.05, 0.05, 0.05)))
    franka = scene.add_entity(gs.morphs.MJCF(file='xml/franka_emika_panda/panda.xml'))
    
    cam = scene.add_camera(GUI=True, fov=70)
    scene.build()

    T = np.eye(4)
    T[:3, :3] = np.array([
        [1,  0,  0],
        [0, -1,  0],
        [0,  0, -1]
    ])
    T[:3, 3] = np.array([0.1, 0.0, 0.1])
    cam.attach(franka.get_link("hand"), T)

    target_quat = np.array([0, 1, 0, 0]) # pointing downwards
    center = np.array([0.5, 0.0, 0.5])
    angular_speed = np.random.random() * 10.0
    r = 0.1

    ee_link = franka.get_link('hand')

    for i in range(1000):
        target_pos = center.copy()
        target_pos[0] += np.cos(i/360*np.pi*angular_speed) * r
        target_pos[1] += np.sin(i/360*np.pi*angular_speed) * r

        q = franka.inverse_kinematics(
            link     = ee_link,
            pos      = target_pos,
            quat     = target_quat,
            rot_mask = [False, False, True], # for demo purpose: only restrict direction of z-axis
        )

        franka.set_qpos(q)
        scene.step()
        cam.render()
        
if __name__ == "__main__":
    main()

Video Demo:

genesis-parallel-camera-mounts.webm

Checklist:

• I read the CONTRIBUTING document.
• I followed the Submitting Code Changes section of CONTRIBUTING document.
• I tagged the title correctly (including BUG FIX/FEATURE/MISC/BREAKING)
• I updated the documentation accordingly or no change is needed.
• I tested my changes and added instructions on how to test it for reviewers.

• I have added tests to cover my changes.
• All new and existing tests passed.

[duburcqa]

Must add a unit test, either as part of test_batched_offscreen_rendering or a completely new test. Just check that it does not fail and the RGB arrays are all non-zeros and significantly different.

[abhijitmajumdar]

Must add a unit test, either as part of test_batched_offscreen_rendering or a completely new test. Just check that it does not fail and the RGB arrays are all non-zeros and significantly different.

@duburcqa Added a separate test. Let me know if that needs changes

[abhijitmajumdar]

Thanks for the review, addressed comments

[duburcqa]

Good job! Ready to merge 👍