def pybulletenv_get_state(env):
    """
    Function used to get state information from PyBullet physics engine.
    :param env:
    :return:
    """
    body_num = env.env._p.getNumBodies()
    # body_info = [env.env._p.getBodyInfo(body_i) for body_i in range(body_num)]
    floor_id, robot_id = 0, 1

    robot_base_pos_ori = env.env._p.getBasePositionAndOrientation(robot_id)
    robot_base_vel = env.env._p.getBaseVelocity(robot_id)

    joint_num = env.env._p.getNumJoints(robot_id)
    joint_state = []
    for joint_i in range(joint_num):
        joint_state.append(env.env._p.getJointState(robot_id, joint_i))

    state = {'body_num': body_num,
             'robot_base_pos_ori': robot_base_pos_ori, 'robot_base_vel': robot_base_vel,
             'joint_num': joint_num, 'joint_state': joint_state}
    return state