Run pre-commit

airo-ugent · Sep 11, 2024 · 3344191 · 3344191
1 parent 6fb404e
commit 3344191
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 100 deletions.
diff --git a/airo_planner/interfaces.py b/airo_planner/interfaces.py
@@ -39,7 +39,7 @@ class SingleArmPlanner(abc.ABC):
     """
 
     def plan_to_joint_configuration(
-            self, start_configuration: JointConfigurationType, goal_configuration: JointConfigurationType
+        self, start_configuration: JointConfigurationType, goal_configuration: JointConfigurationType
     ) -> JointPathType:
         """Plan a path from a start configuration to a goal configuration.
 
@@ -62,7 +62,7 @@ def plan_to_joint_configuration(
         raise NotImplementedError("This planner has not implemented planning to joint configurations (yet).")
 
     def plan_to_tcp_pose(
-            self, start_configuration: JointConfigurationType, tcp_pose: HomogeneousMatrixType
+        self, start_configuration: JointConfigurationType, tcp_pose: HomogeneousMatrixType
     ) -> JointPathType:
         raise NotImplementedError("This planner has not implemented planning to TCP poses (yet).")
 
@@ -76,11 +76,11 @@ class DualArmPlanner(abc.ABC):
     """
 
     def plan_to_joint_configuration(
-            self,
-            start_configuration_left: JointConfigurationType,
-            start_configuration_right: JointConfigurationType,
-            goal_configuration_left: JointConfigurationType | None,
-            goal_configuration_right: JointConfigurationType | None,
+        self,
+        start_configuration_left: JointConfigurationType,
+        start_configuration_right: JointConfigurationType,
+        goal_configuration_left: JointConfigurationType | None,
+        goal_configuration_right: JointConfigurationType | None,
     ) -> JointPathType:
         """Plan a path from a start configurations to a goal configurations.
 
@@ -110,10 +110,10 @@ def plan_to_joint_configuration(
         raise NotImplementedError("This planner has not implemented planning to joint configurations (yet).")
 
     def plan_to_tcp_pose(
-            self,
-            start_configuration_left: JointConfigurationType,
-            start_configuration_right: JointConfigurationType,
-            tcp_pose_left: HomogeneousMatrixType | None,
-            tcp_pose_right: HomogeneousMatrixType | None,
+        self,
+        start_configuration_left: JointConfigurationType,
+        start_configuration_right: JointConfigurationType,
+        tcp_pose_left: HomogeneousMatrixType | None,
+        tcp_pose_right: HomogeneousMatrixType | None,
     ) -> JointPathType:
         raise NotImplementedError("This planner has not implemented planning to TCP poses (yet).")
diff --git a/airo_planner/ompl/mobile_platform_ompl_planner.py b/airo_planner/ompl/mobile_platform_ompl_planner.py
@@ -1,8 +1,8 @@
 import numpy as np
-from airo_typing import Vector3DType, JointPathType, JointConfigurationCheckerType
+from airo_typing import JointConfigurationCheckerType, JointPathType, Vector3DType
 from loguru import logger
 
-from airo_planner import create_simple_setup, state_to_ompl, NoPathFoundError, solve_and_smooth_path
+from airo_planner import NoPathFoundError, create_simple_setup, solve_and_smooth_path, state_to_ompl
 from airo_planner.interfaces import MobilePlatformPlanner
 
 
@@ -21,10 +21,7 @@ def __init__(self, is_state_valid_fn: JointConfigurationCheckerType):
         self.is_state_valid_fn = is_state_valid_fn
 
         # TODO: Allow user to set these bounds.
-        joint_bounds = (
-            np.array([-10.0, -10.0, -4 * np.pi]),
-            np.array([10.0, 10.0, 4 * np.pi])
-        )
+        joint_bounds = (np.array([-10.0, -10.0, -4 * np.pi]), np.array([10.0, 10.0, 4 * np.pi]))
         self._simple_setup = create_simple_setup(self.is_state_valid_fn, joint_bounds)
 
     def plan_to_pose(self, start_pose: Vector3DType, goal_pose: Vector3DType) -> JointPathType:

diff --git a/notebooks/04_mobile_platform_planning.ipynb b/notebooks/04_mobile_platform_planning.ipynb
@@ -20,27 +20,9 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 1,
+   "execution_count": null,
    "metadata": {},
-   "outputs": [
-    {
-     "name": "stderr",
-     "output_type": "stream",
-     "text": [
-      "INFO:drake:Meshcat listening for connections at http://localhost:7000\n"
-     ]
-    },
-    {
-     "data": {
-      "text/plain": [
-       "MobilePlatformWithSingleArmScene(robot_diagram=<pydrake.planning.RobotDiagram object at 0x7ff8fb925e70>, mobile_platform_index=ModelInstanceIndex(2), arm_index=ModelInstanceIndex(12), gripper_index=ModelInstanceIndex(13), meshcat=<pydrake.geometry.Meshcat object at 0x7ff8f72fae70>)"
-      ]
-     },
-     "execution_count": 1,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
+   "outputs": [],
    "source": [
     "import numpy as np\n",
     "from pydrake.planning import RobotDiagramBuilder, SceneGraphCollisionChecker\n",
@@ -83,17 +65,9 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 2,
+   "execution_count": null,
    "metadata": {},
-   "outputs": [
-    {
-     "name": "stderr",
-     "output_type": "stream",
-     "text": [
-      "INFO:drake:Allocating contexts to support implicit context parallelism 8\n"
-     ]
-    }
-   ],
+   "outputs": [],
    "source": [
     "collision_checker = SceneGraphCollisionChecker(\n",
     "    model=scene.robot_diagram,\n",
@@ -110,47 +84,25 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 3,
+   "execution_count": null,
    "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/plain": [
-       "3"
-      ]
-     },
-     "execution_count": 3,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
+   "outputs": [],
    "source": [
     "scene.robot_diagram.plant().num_positions()"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 4,
+   "execution_count": null,
    "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/plain": [
-       "(True, False)"
-      ]
-     },
-     "execution_count": 4,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
+   "outputs": [],
    "source": [
     "collision_checker.CheckConfigCollisionFree([1.0, 0.0, 0]), collision_checker.CheckConfigCollisionFree([1.0, 0.0, np.pi/2])"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 5,
+   "execution_count": null,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -182,7 +134,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 6,
+   "execution_count": null,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -204,7 +156,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 7,
+   "execution_count": null,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -217,29 +169,9 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 8,
+   "execution_count": null,
    "metadata": {},
-   "outputs": [
-    {
-     "name": "stderr",
-     "output_type": "stream",
-     "text": [
-      "\u001b[32m2024-08-01 10:03:31.391\u001b[0m | \u001b[32m\u001b[1mSUCCESS \u001b[0m | \u001b[36mairo_planner.ompl.mobile_platform_ompl_planner\u001b[0m:\u001b[36mplan_to_pose\u001b[0m:\u001b[36m43\u001b[0m - \u001b[32m\u001b[1mSuccessfully found path (with 65 waypoints).\u001b[0m\n"
-     ]
-    },
-    {
-     "name": "stdout",
-     "output_type": "stream",
-     "text": [
-      "Debug:   RRTConnect: Planner range detected to be 7.567443\n",
-      "Info:    RRTConnect: Starting planning with 1 states already in datastructure\n",
-      "Info:    RRTConnect: Created 5 states (2 start + 3 goal)\n",
-      "Info:    Solution found in 0.021869 seconds\n",
-      "Info:    SimpleSetup: Path simplification took 0.117797 seconds and changed from 4 to 3 states\n",
-      "(65, 3)\n"
-     ]
-    }
-   ],
+   "outputs": [],
    "source": [
     "path = planner.plan_to_pose(\n",
     "    start_pose, goal_pose\n",
@@ -249,7 +181,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 9,
+   "execution_count": null,
    "metadata": {},
    "outputs": [],
    "source": [