def run_motion_planner():
    result = None
    openrave_manager = OpenraveManager(
        config['openrave_rl']['segment_validity_step'],
        PotentialPoint.from_config(config))
    for start_joints, goal_joints, workspace_id, _ in queries:
        params_file_path = image_cache.items[workspace_id].full_filename
        openrave_manager.set_params(params_file_path)
        for i in range(repeat):
            start_time = datetime.datetime.now()
            traj = openrave_manager.plan(start_joints, goal_joints, None)
            # assert traj is not None
            end_time = datetime.datetime.now()
            time_diff = end_time - start_time
            if result is None:
                result = time_diff
            else:
                result += time_diff
    return result
class OpenraveTrajectoryGenerator:
    def __init__(self, config):
        self.action_step_size = config["openrave_rl"]["action_step_size"]
        self.goal_sensitivity = config["openrave_rl"]["goal_sensitivity"]
        self.challenging_trajectories_only = config["openrave_planner"][
            "challenging_trajectories_only"]
        self.planner_iterations_start = config["openrave_planner"][
            "planner_iterations_start"]
        self.planner_iterations_increase = config["openrave_planner"][
            "planner_iterations_increase"]
        self.planner_iterations_decrease = config["openrave_planner"][
            "planner_iterations_decrease"]
        self.max_planner_iterations = self.planner_iterations_start

        self.openrave_manager = OpenraveManager(
            config["openrave_rl"]["segment_validity_step"],
            PotentialPoint.from_config(config),
        )

    def is_below_goal_sensitivity(self, start_joints, goal_joints):
        start_pose = self.openrave_manager.get_target_pose(start_joints)
        goal_pose = self.openrave_manager.get_target_pose(goal_joints)
        pose_distance = np.linalg.norm(
            np.array(start_pose) - np.array(goal_pose))
        return pose_distance < self.goal_sensitivity

    def find_random_trajectory_single_try(self):
        # select at random
        start_joints = self.openrave_manager.get_random_joints({0: 0.0})
        goal_joints = self.openrave_manager.get_random_joints({0: 0.0})
        # if the start and goal are too close, re-sample
        if self.is_below_goal_sensitivity(start_joints, goal_joints):
            return None
        start_pose = self.openrave_manager.get_target_pose(start_joints)
        goal_pose = self.openrave_manager.get_target_pose(goal_joints)
        # valid region:
        if not self._is_valid_region(start_pose, goal_pose):
            return None
        # trajectories that must cross an obstacle
        if self.challenging_trajectories_only and not self._is_challenging(
                start_pose, goal_pose):
            return None
        traj = self.openrave_manager.plan(start_joints, goal_joints,
                                          self.max_planner_iterations)
        return traj

    def find_random_trajectory(self):
        # lower_size = 0.0  # when doing curriculum, this this is the lowest possible distance between start and goal
        while True:
            traj = self.find_random_trajectory_single_try()
            if traj is None:
                # if failed to plan, give more power
                self.max_planner_iterations += self.planner_iterations_increase
            elif (self.max_planner_iterations > self.planner_iterations_start +
                  self.planner_iterations_decrease):
                # if plan was found, maybe we need less iterations
                self.max_planner_iterations -= self.planner_iterations_decrease
                return self.split_trajectory(traj, self.action_step_size)

    @staticmethod
    def _is_valid_region(start_pose, goal_pose):
        return start_pose[1] > 0.0 and goal_pose[1] > 0.0

    def _is_challenging(self, start_pose, goal_pose):
        workspace_params = self.openrave_manager.loaded_params
        if workspace_params is None or workspace_params.number_of_obstacles == 0:
            return True
        # check if the distance from any obstacle is smaller that the start-goal-distance
        start = np.array(start_pose)
        goal = np.array(goal_pose)
        start_goal_distance = np.linalg.norm(start - goal)
        for i in range(workspace_params.number_of_obstacles):
            obstacle = np.array([
                workspace_params.centers_position_x[i],
                workspace_params.centers_position_z[i],
            ])
            start_obstacle_distance = np.linalg.norm(start - obstacle)
            goal_obstacle_distance = np.linalg.norm(goal - obstacle)
            if (start_obstacle_distance < start_goal_distance
                    and goal_obstacle_distance < start_goal_distance):
                return True
        # all tests failed
        return False

    @staticmethod
    def split_trajectory(trajectory, action_step_size):
        res = [tuple(trajectory[0])]
        for i in range(len(trajectory) - 1):
            current_step = np.array(trajectory[i])
            next_step = np.array(trajectory[i + 1])
            difference = next_step - current_step
            difference_norm = np.linalg.norm(difference)
            if difference_norm < action_step_size:
                # if smaller than allowed step just append the next step
                res.append(tuple(trajectory[i + 1]))
                continue
            scaled_step = (action_step_size / difference_norm) * difference
            steps = []
            for alpha in range(
                    int(np.floor(difference_norm / action_step_size))):
                processed_step = current_step + (1 + alpha) * scaled_step
                steps.append(processed_step)
            # we probably have a leftover section, append it to res
            last_step_difference = np.linalg.norm(steps[-1] - next_step)
            if last_step_difference > 0.0:
                steps.append(next_step)
            # append to path
            res += [tuple(s) for s in steps]
        return res