    def __init__(self, robot, obstacles = [], allow_collision_links=[], allow_collision_robot_link_pairs=[]) -> None:
        '''
        Args
            robot: A PbOMPLRobot instance.
            obstacles: list of obstacle ids. Optional.
        '''
        self.robot = robot
        self.robot_id = robot.id
        self.obstacles = obstacles
        self.allow_collision_links = allow_collision_links
        self.allow_collision_robot_link_pairs = allow_collision_robot_link_pairs
        print("motion planning obstables: ", self.obstacles)
        print("robot allow collision links: ", allow_collision_links)

        self.space = PbStateSpace(robot.num_dim)
        self.start = copy.deepcopy(self.robot.get_cur_state())

        bounds = ob.RealVectorBounds(robot.num_dim)
        joint_bounds = self.robot.joint_bounds
        for i, bound in enumerate(joint_bounds):
            bounds.setLow(i, bound[0])
            bounds.setHigh(i, bound[1])
        self.space.setBounds(bounds)

        self.ss = og.SimpleSetup(self.space)
        self.ss.setStateValidityChecker(ob.StateValidityCheckerFn(self.is_state_valid))
        self.si = self.ss.getSpaceInformation()
        # self.si.setStateValidityCheckingResolution(0.005)
        # self.collision_fn = pb_utils.get_collision_fn(self.robot_id, self.robot.joint_idx, self.obstacles, [], True, set(),
        #                                                 custom_limits={}, max_distance=0, allow_collision_links=[])

        self.set_obstacles(obstacles)
        self.set_planner("RRT") # RRT by default
Args
robot: A PbOMPLRobot instance.
obstacles: list of obstacle ids. Optional.
pybullet_ompl/pb_ompl.py