diff --git a/airo_planner/ompl/utilities.py b/airo_planner/ompl/utilities.py index 44c37ff..a64a5b8 100644 --- a/airo_planner/ompl/utilities.py +++ b/airo_planner/ompl/utilities.py @@ -46,13 +46,6 @@ def bounds_to_ompl(joint_bounds: JointBoundsType) -> ob.RealVectorBounds: return bounds -def allocDeterministicStateSampler(space: ob.StateSpace) -> ob.StateSampler: - from loguru import logger - - logger.info("Allocating RealVectorDeterministicStateSampler") - return ob.RealVectorDeterministicStateSampler(space) - - def create_simple_setup( is_state_valid_fn: JointConfigurationCheckerType, joint_bounds: JointBoundsType ) -> og.SimpleSetup: @@ -72,9 +65,6 @@ def create_simple_setup( is_state_valid_ompl = function_to_ompl(is_state_valid_fn, degrees_of_freedom) - space.setStateSamplerAllocator(ob.StateSamplerAllocator(allocDeterministicStateSampler)) - # space.allocStateSampler() - # Configure the SimpleSetup object simple_setup = og.SimpleSetup(space) simple_setup.setStateValidityChecker(ob.StateValidityCheckerFn(is_state_valid_ompl)) diff --git a/notebooks/01_planning_to_joint_configurations.ipynb b/notebooks/01_planning_to_joint_configurations.ipynb index 65027a6..4e011f8 100644 --- a/notebooks/01_planning_to_joint_configurations.ipynb +++ b/notebooks/01_planning_to_joint_configurations.ipynb @@ -122,15 +122,6 @@ "animate_joint_trajectory(scene.meshcat, scene.robot_diagram, scene.arm_index, trajectory)" ] }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# planner._simple_setup.getSpaceInformation().ValidStateSamplerAllocator()" - ] - }, { "cell_type": "code", "execution_count": null, diff --git a/notebooks/02_planning_to_tcp_poses.ipynb b/notebooks/02_planning_to_tcp_poses.ipynb index 3088a23..fc5ef5c 100644 --- a/notebooks/02_planning_to_tcp_poses.ipynb +++ b/notebooks/02_planning_to_tcp_poses.ipynb @@ -291,105 +291,6 @@ "print(f\"\\nLength of path that planner returned: {calculate_joint_path_length(path):.2f}\")" ] }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "valid_sampler = planner._simple_setup.getSpaceInformation().allocValidStateSampler()\n", - "valid_sampler" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "from ompl import base as ob\n", - "from ompl import util as ou\n", - "from loguru import logger\n", - "\n", - "class DeterministicValidStateSampler(ob.ValidStateSampler):\n", - " def __init__(self, space_information: ob.SpaceInformation, random_seed: int = 1):\n", - " logger.info(\"Creating DeterministicValidStateSampler\")\n", - " super(DeterministicValidStateSampler, self).__init__(space_information)\n", - " self.rng_ = ou.RNG(random_seed)\n", - "\n", - "\n", - "def allocDeterministicValidStateSampler(space_information: ob.SpaceInformation) -> ob.ValidStateSampler:\n", - " logger.info(\"Allocating DeterministicValidStateSampler\")\n", - " return DeterministicValidStateSampler(space_information)\n", - "\n", - "\n", - "space_information = planner._simple_setup.getSpaceInformation()\n", - "DeterministicValidStateSampler(space_information)\n", - "allocDeterministicValidStateSampler(space_information)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "ob.RealVectorDeterministicStateSampler(planner._simple_setup.getStateSpace())" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# def allocDeterministicStateSampler(space: ob.StateSpace) -> ob.StateSampler:\n", - "# return ob.RealVectorDeterministicStateSampler(space)\n", - "\n", - "# planner._simple_setup.getStateSpace().setStateSamplerAllocator(ob.StateSamplerAllocator(allocDeterministicStateSampler))" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "from airo_planner.ompl.utilities import create_simple_setup\n", - "import ompl.geometric as og\n", - "\n", - "planner._simple_setup = create_simple_setup(planner.is_state_valid_fn, planner.joint_bounds)\n", - "# space_information = planner._simple_setup.getSpaceInformation()\n", - "# space_information.clearValidStateSamplerAllocator()\n", - "# space_information.setValidStateSamplerAllocator(ob.ValidStateSamplerAllocator(allocDeterministicValidStateSampler))\n", - "# space_information.allocValidStateSampler()\n", - "\n", - "# rrt_connect = og.RRTConnect(space_information)\n", - "# planner._simple_setup.setPlanner(rrt_connect)\n", - "\n", - "path = planner.plan_to_tcp_pose(start_joints, goal_pose)\n", - "calculate_joint_path_length(path)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "planner._simple_setup = create_simple_setup(planner.is_state_valid_fn, planner.joint_bounds)\n", - "# space_information = planner._simple_setup.getSpaceInformation()\n", - "# space_information.clearValidStateSamplerAllocator()\n", - "# space_information.setValidStateSamplerAllocator(ob.ValidStateSamplerAllocator(allocDeterministicValidStateSampler))\n", - "# space_information.allocValidStateSampler()\n", - "\n", - "# rrt_connect = og.RRTConnect(space_information)\n", - "# planner._simple_setup.setPlanner(rrt_connect)\n", - "\n", - "path = planner.plan_to_tcp_pose(start_joints, goal_pose)\n", - "calculate_joint_path_length(path)" - ] - }, { "cell_type": "markdown", "metadata": {},