def setup_scene(rbt, config):
    if (config['with_ground'] is True):
        AddFlatTerrainToWorld(rbt)
    for (i, instance_config) in enumerate(config['instances']):
        add_single_instance_to_rbt(rbt, config, instance_config, i, floating_base_type=FloatingBaseType.kFixed)
    camera_link = RigidBody()
    camera_link.set_name('camera_link')
    camera_link.set_spatial_inertia(np.eye(6))
    camera_link.add_joint(rbt.world(), RollPitchYawFloatingJoint('camera_floating_base', np.eye(4)))
    rbt.add_rigid_body(camera_link)
    camera_frame = RigidBodyFrame(name='rgbd_camera_frame', body=camera_link, xyz=[0.0, 0.0, 0.0], rpy=[0.0, 0.0, 0.0])
    rbt.addFrame(camera_frame)
    rbt.compile()
def setup_scene(rbt, config):
    if (config['with_ground'] is True):
        AddFlatTerrainToWorld(rbt)
    for (i, instance_config) in enumerate(config['instances']):
        add_single_instance_to_rbt(rbt, config, instance_config, i, floating_base_type=FloatingBaseType.kFixed)
    camera_link = RigidBody()
    camera_link.set_name('camera_link')
    camera_link.set_spatial_inertia(np.eye(6))
    camera_link.add_joint(rbt.world(), RollPitchYawFloatingJoint('camera_floating_base', np.eye(4)))
    rbt.add_rigid_body(camera_link)
    camera_frame = RigidBodyFrame(name='rgbd_camera_frame', body=camera_link, xyz=[0.0, 0.0, 0.0], rpy=[0.0, 0.0, 0.0])
    rbt.addFrame(camera_frame)
    rbt.compile()