start for single arm planning notebook

README.md

@@ -2,8 +2,8 @@
 Python package for single and dual robot arm motion planning.
 
 **Key motivation:**
-  - 🔗 **Bridge** the gap between **OMPL**'s the powerful (but robot-agnostic) sampling-based planners and **Drake**'s collision checking for robots.
-  - 🦾 **Standardize** and add other features taylored to robotic arm motion planning such as joint limits can planning to TCP poses.
+  - 🔗 **Bridge** the gap between [**OMPL**](https://ompl.kavrakilab.org/)'s powerful (but robot-agnostic) sampling-based planners and [**Drake**](https://drake.mit.edu/)'s collision checking for robots.
+  - 🦾 **Standardize** and add other features taylored to robotic arm motion planning such as joint limits and planning to TCP poses.
 
 ## Overview 🧾
 
@@ -12,7 +12,7 @@ Python package for single and dual robot arm motion planning.
     - `SingleArmPlanner`
     - `DualArmPlanner`
   - 🔌 **Implementations:** reliable and well-tested implementations of these interfaces.
-    - Comimg soon: OMPL for single (TODO: and dual arm planning)
+    - **🚧 Coming soon:** OMPL for single (and dual arm) planning
 
 **Design goals:**
   - ⚓ **Robustness and stability:** provide an *off-the-shelf* motion planner that supports research by reliably covering most (not *all*) use cases at our labs, prioritizing dependability over niche, cutting-edge features.

airo_planner/__init__.py

@@ -3,25 +3,29 @@
 # but for building and many toolings, you still need to have __init__ files (at least in the root of the package).
 # e.g. if you remove this init file and try to build with pip install .
 # you won't be able to import the dummy module.
-from airo_planner.ompl.state_space import (  # isort:skip
-    function_numpy_to_ompl,
-    numpy_to_ompl_state,
-    ompl_path_to_numpy,
-    ompl_state_to_numpy,
+from airo_planner.ompl.utilities import (  # isort:skip
+    function_to_ompl,
+    state_to_ompl,
+    path_from_ompl,
+    state_from_ompl,
+    bounds_to_ompl,
+    JointBoundsType,
 )
 
 from airo_planner.interfaces import DualArmPlanner, SingleArmPlanner  # isort:skip
-from airo_planner.ompl.single_arm_ompl_planner import InverseKinematicsType, SingleArmOmplPlanner  # isort:skip
+from airo_planner.ompl.single_arm_ompl_planner import InverseKinematicsFunctionType, SingleArmOmplPlanner  # isort:skip
 from airo_planner.ompl.dual_arm_ompl_planner import DualArmOmplPlanner  # isort:skip
 
 __all__ = [
     "SingleArmPlanner",
     "DualArmPlanner",
     "SingleArmOmplPlanner",
     "DualArmOmplPlanner",
-    "InverseKinematicsType",
-    "ompl_state_to_numpy",
-    "numpy_to_ompl_state",
-    "ompl_path_to_numpy",
-    "function_numpy_to_ompl",
+    "InverseKinematicsFunctionType",
+    "state_from_ompl",
+    "state_to_ompl",
+    "path_from_ompl",
+    "function_to_ompl",
+    "bounds_to_ompl",
+    "JointBoundsType",
 ]

airo_planner/interfaces.py

@@ -1,28 +1,34 @@
 import abc
-from typing import List, Optional, Tuple, Union
+from typing import List, Tuple, Union
 
-from airo_typing import HomogeneousMatrixType, JointConfigurationType
+from airo_typing import HomogeneousMatrixType, JointConfigurationType, JointPathType
 
 
 class SingleArmPlanner(abc.ABC):
     """Base class that defines an interface for single-arm motion planners.
 
     The idea is that the custom settings for each motion planner are provided
-    through the constructor, and from then on all motion planners can be used
-    in the same way, e.g. for bemchmarking.
+    through the constructor. After creation the motion planner can then be
+
+      and from then on all motion planners can be used
+    in the same way, e.g. for benchmarking.
     """
 
     @abc.abstractmethod
     def plan_to_joint_configuration(
         self,
         start_configuration: JointConfigurationType,
         goal_configuration: JointConfigurationType,
-    ) -> Union[List[JointConfigurationType], None]:
+    ) -> JointPathType | None:
         """Plan a path from a start configuration to a goal configuration.
 
+        Note that this path is not guarenteed to be dense, i.e. the distance
+        between consecutive configurations in the path may be large. For this
+        reason, you might need to post-process these path before executing it.
+
         Args:
-            start_configuration: The start configuration.
-            goal_configuration: The goal configuration.
+            start_configuration: The start joint configuration.
+            goal_configuration: The goal joint configuration.
 
         Returns:
             A discretized path from the start configuration to the goal
@@ -31,12 +37,11 @@ def plan_to_joint_configuration(
         raise NotImplementedError
 
     @abc.abstractmethod
-    def plan_to_tcp_pose(  # noqa: C901
+    def plan_to_tcp_pose(
         self,
         start_configuration: JointConfigurationType,
         tcp_pose_in_base: HomogeneousMatrixType,
-        desirable_goal_configurations: Optional[List[JointConfigurationType]] = None,
-    ) -> List[JointConfigurationType] | None:
+    ) -> JointPathType | None:
         """TODO"""
         raise NotImplementedError
 

airo_planner/ompl/dual_arm_ompl_planner.py

@@ -8,11 +8,11 @@
 
 from airo_planner import (
     DualArmPlanner,
-    InverseKinematicsType,
+    InverseKinematicsFunctionType,
     SingleArmOmplPlanner,
-    function_numpy_to_ompl,
-    numpy_to_ompl_state,
-    ompl_path_to_numpy,
+    function_to_ompl,
+    path_from_ompl,
+    state_to_ompl,
 )
 
 DualJointConfigurationCheckerType = Callable[[np.ndarray], bool]
@@ -22,8 +22,8 @@ class DualArmOmplPlanner(DualArmPlanner):
     def __init__(
         self,
         is_state_valid_fn: DualJointConfigurationCheckerType,
-        inverse_kinematics_left_fn: Optional[InverseKinematicsType] = None,
-        inverse_kinematics_right_fn: Optional[InverseKinematicsType] = None,
+        inverse_kinematics_left_fn: Optional[InverseKinematicsFunctionType] = None,
+        inverse_kinematics_right_fn: Optional[InverseKinematicsFunctionType] = None,
         joint_bounds_left: Optional[Tuple[JointConfigurationType, JointConfigurationType]] = None,
         joint_bounds_right: Optional[Tuple[JointConfigurationType, JointConfigurationType]] = None,
         max_planning_time: float = 5.0,
@@ -74,7 +74,7 @@ def _create_simple_setup_dual_arm(self) -> og.SimpleSetup:
                 bounds.setHigh(i, self.joint_bounds_right[1][i - 6])
         space.setBounds(bounds)
 
-        is_state_valid_ompl = function_numpy_to_ompl(self.is_state_valid_fn, self.degrees_of_freedom)
+        is_state_valid_ompl = function_to_ompl(self.is_state_valid_fn, self.degrees_of_freedom)
 
         simple_setup = og.SimpleSetup(space)
         simple_setup.setStateValidityChecker(ob.StateValidityCheckerFn(is_state_valid_ompl))
@@ -100,8 +100,8 @@ def _set_start_and_goal_configurations(
         space = self._simple_setup.getStateSpace()
         start_configuration = np.concatenate([start_configuration_left, start_configuration_right])
         goal_configuration = np.concatenate([goal_configuration_left, goal_configuration_right])
-        start_state = numpy_to_ompl_state(start_configuration, space)
-        goal_state = numpy_to_ompl_state(goal_configuration, space)
+        start_state = state_to_ompl(start_configuration, space)
+        goal_state = state_to_ompl(goal_configuration, space)
         self._simple_setup.setStartAndGoalStates(start_state, goal_state)
 
         # Replace single arm planners for the left and right arm
@@ -167,7 +167,7 @@ def _plan_to_joint_configuration_dual_arm(
 
         self._path_length_dual = path.length()
 
-        path_numpy = ompl_path_to_numpy(path, self.degrees_of_freedom)
+        path_numpy = path_from_ompl(path, self.degrees_of_freedom)
         path_tuple = [(state[:6], state[6:]) for state in path_numpy]
         return path_tuple