def add_single_instance_to_rbt(rbt, config, instance_config, i, floating_base_type=FloatingBaseType.kRollPitchYaw):
    class_name = instance_config['class']
    if (class_name not in config['objects'].keys()):
        raise ValueError(('Class %s not in classes.' % class_name))
    if (len(instance_config['pose']) != 6):
        raise ValueError(('Class %s has pose size != 6. Use RPY plz' % class_name))
    frame = RigidBodyFrame(('%s_%d' % (class_name, i)), rbt.world(), instance_config['pose'][0:3], instance_config['pose'][3:6])
    model_path = config['objects'][class_name]['model_path']
    (_, extension) = os.path.splitext(model_path)
    if (extension == '.urdf'):
        AddModelInstanceFromUrdfFile(model_path, floating_base_type, frame, rbt)
    elif (extension == '.sdf'):
        AddModelInstancesFromSdfString(open(model_path).read(), floating_base_type, frame, rbt)
    else:
        raise ValueError(('Class %s has non-sdf and non-urdf model name.' % class_name))
def add_single_instance_to_rbt(rbt, config, instance_config, i, floating_base_type=FloatingBaseType.kRollPitchYaw):
    class_name = instance_config['class']
    if (class_name not in config['objects'].keys()):
        raise ValueError(('Class %s not in classes.' % class_name))
    if (len(instance_config['pose']) != 6):
        raise ValueError(('Class %s has pose size != 6. Use RPY plz' % class_name))
    frame = RigidBodyFrame(('%s_%d' % (class_name, i)), rbt.world(), instance_config['pose'][0:3], instance_config['pose'][3:6])
    model_path = config['objects'][class_name]['model_path']
    (_, extension) = os.path.splitext(model_path)
    if (extension == '.urdf'):
        AddModelInstanceFromUrdfFile(model_path, floating_base_type, frame, rbt)
    elif (extension == '.sdf'):
        AddModelInstancesFromSdfString(open(model_path).read(), floating_base_type, frame, rbt)
    else:
        raise ValueError(('Class %s has non-sdf and non-urdf model name.' % class_name))