diff --git a/airo_planner/__init__.py b/airo_planner/__init__.py index 71e3849..d8e64a2 100644 --- a/airo_planner/__init__.py +++ b/airo_planner/__init__.py @@ -3,7 +3,12 @@ # 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.joint_bounds import JointBoundsType, concatenate_joint_bounds, uniform_symmetric_joint_bounds +from airo_planner.joint_bounds import ( + JointBoundsType, + concatenate_joint_bounds, + is_within_bounds, + uniform_symmetric_joint_bounds, +) from airo_planner.path_conversions import stack_joints from airo_planner.selection.goal_selection import ( filter_with_distance_to_configurations, @@ -32,6 +37,7 @@ "SingleArmOmplPlanner", "DualArmOmplPlanner", "JointBoundsType", + "is_within_bounds", "uniform_symmetric_joint_bounds", "concatenate_joint_bounds", "state_from_ompl", diff --git a/airo_planner/exceptions.py b/airo_planner/exceptions.py new file mode 100644 index 0000000..85df431 --- /dev/null +++ b/airo_planner/exceptions.py @@ -0,0 +1,8 @@ +class PlannerError(RuntimeError): + """Base class for all things that can go wrong during planning. + Mostly intended for problems that can happen, but are not per se + the user's fault, e.g. no valid IK solutions, no valid paths etc. + + Note that not all planner problems need to be a subclass of this + error, e.g. you can still use ValueError, AttributeError etc. + """ diff --git a/airo_planner/interfaces.py b/airo_planner/interfaces.py index 04c7709..a1631af 100644 --- a/airo_planner/interfaces.py +++ b/airo_planner/interfaces.py @@ -7,10 +7,8 @@ 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. 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. + through the constructor. After creation all motion planners can then be + used in the same way, e.g. for benchmarking. """ def plan_to_joint_configuration( @@ -32,15 +30,14 @@ def plan_to_joint_configuration( A discretized path from the start configuration to the goal configuration. If no solution could be found, then None is returned. """ - raise NotImplementedError + raise NotImplementedError("This planner has not implemented planning to joint configurations (yet).") def plan_to_tcp_pose( self, start_configuration: JointConfigurationType, - tcp_pose_in_base: HomogeneousMatrixType, + tcp_pose: HomogeneousMatrixType, ) -> JointPathType | None: - """TODO""" - raise NotImplementedError + raise NotImplementedError("This planner has not implemented planning to TCP poses (yet).") class DualArmPlanner(abc.ABC): @@ -79,14 +76,13 @@ def plan_to_joint_configuration( the start_configuration will simply be repeated in the path for that arm. If no solution could be found, then None is returned. """ - raise NotImplementedError + 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_in_base: HomogeneousMatrixType | None, - tcp_pose_right_in_base: HomogeneousMatrixType | None, + tcp_pose_left: HomogeneousMatrixType | None, + tcp_pose_right: HomogeneousMatrixType | None, ) -> JointPathType | None: - """TODO""" - raise NotImplementedError + raise NotImplementedError("This planner has not implemented planning to TCP poses (yet).") diff --git a/airo_planner/joint_bounds.py b/airo_planner/joint_bounds.py index 07121ef..fbb3f18 100644 --- a/airo_planner/joint_bounds.py +++ b/airo_planner/joint_bounds.py @@ -6,6 +6,21 @@ JointBoundsType = tuple[JointConfigurationType, JointConfigurationType] +def is_within_bounds(joint_configuration: JointConfigurationType, joint_bounds: JointBoundsType) -> bool: + """Checks if a joint configuration is within the given bounds. + + Args: + joint_configuration: The joint configuration to check. + joint_bounds: A tuple containing the lower and upper bounds for each joint. + + Returns: + True if the joint configuration is within the bounds, False otherwise. + """ + lower_bounds, upper_bounds = joint_bounds + is_within = np.all(lower_bounds <= joint_configuration) and np.all(joint_configuration <= upper_bounds) + return bool(is_within) + + def uniform_symmetric_joint_bounds(degrees_of_freedom: int, value: float = 2 * np.pi) -> JointBoundsType: """Creates joint bounds that are the same for each joint and where the min is the negative of the max. diff --git a/airo_planner/ompl/dual_arm_ompl_planner.py b/airo_planner/ompl/dual_arm_ompl_planner.py index 512a5a4..f28955e 100644 --- a/airo_planner/ompl/dual_arm_ompl_planner.py +++ b/airo_planner/ompl/dual_arm_ompl_planner.py @@ -1,5 +1,6 @@ import numpy as np from airo_typing import ( + HomogeneousMatrixType, InverseKinematicsFunctionType, JointConfigurationCheckerType, JointConfigurationType, @@ -209,14 +210,16 @@ def plan_to_joint_configuration( ) return path - # def plan_to_tcp_pose( - # self, - # start_configuration_left: HomogeneousMatrixType, - # start_configuration_right: HomogeneousMatrixType, - # tcp_pose_left_in_base: HomogeneousMatrixType | None, - # tcp_pose_right_in_base: HomogeneousMatrixType | None, - # ) -> JointPathType | None: - # pass + def plan_to_tcp_pose( + self, + start_configuration_left: HomogeneousMatrixType, + start_configuration_right: HomogeneousMatrixType, + tcp_pose_left: HomogeneousMatrixType | None, + tcp_pose_right: HomogeneousMatrixType | None, + ) -> JointPathType | None: + if tcp_pose_left is None and tcp_pose_right is None: + raise ValueError("A goal TCP pose must be specified for at least one of the arms.") + return None # def _plan_to_tcp_pose_left_arm_only( # self, diff --git a/airo_planner/ompl/single_arm_ompl_planner.py b/airo_planner/ompl/single_arm_ompl_planner.py index c566e28..f65eb91 100644 --- a/airo_planner/ompl/single_arm_ompl_planner.py +++ b/airo_planner/ompl/single_arm_ompl_planner.py @@ -16,11 +16,13 @@ SingleArmPlanner, choose_shortest_path, create_simple_setup, + is_within_bounds, solve_and_smooth_path, state_to_ompl, + uniform_symmetric_joint_bounds, ) -# TODO move this to airo_typing +# TODO move this to airo_typing? JointConfigurationsModifierType = Callable[[List[JointConfigurationType]], List[JointConfigurationType]] JointPathChooserType = Callable[[List[JointPathType]], JointPathType] @@ -70,7 +72,7 @@ def __init__( # Planning parameters self._degrees_of_freedom = degrees_of_freedom if joint_bounds is None: - self.joint_bounds = np.full(degrees_of_freedom, -2 * np.pi), np.full(degrees_of_freedom, 2 * np.pi) + self.joint_bounds = uniform_symmetric_joint_bounds(self._degrees_of_freedom) else: self.joint_bounds = joint_bounds @@ -90,6 +92,11 @@ def _set_start_and_goal_configurations( raise ValueError("Planner was given an invalid goal configuration.") # TODO check joint bounds as well + if not is_within_bounds(start_configuration, self.joint_bounds): + raise ValueError("Start configuration is not within the joint bounds.") + + if not is_within_bounds(goal_configuration, self.joint_bounds): + raise ValueError("Goal configuration is not within the joint bounds.") space = self._simple_setup.getStateSpace() start_state = state_to_ompl(start_configuration, space) @@ -111,13 +118,13 @@ def plan_to_joint_configuration( def plan_to_tcp_pose( # noqa: C901 self, start_configuration: JointConfigurationType, - tcp_pose_in_base: HomogeneousMatrixType, + tcp_pose: HomogeneousMatrixType, ) -> JointPathType | None: + if self.inverse_kinematics_fn is None: - logger.warning("Planning to TCP pose attempted but inverse_kinematics_fn was provided, returing None.") - return None + raise AttributeError("Inverse kinematics function is required for planning to TCP poses.") - ik_solutions = self.inverse_kinematics_fn(tcp_pose_in_base) + ik_solutions = self.inverse_kinematics_fn(tcp_pose) ik_solutions = [solution.squeeze() for solution in ik_solutions] self._ik_solutions = ik_solutions # Save for debugging @@ -145,7 +152,6 @@ def plan_to_tcp_pose( # noqa: C901 else: logger.info(f"Found {len(ik_solutions_valid)}/{len(ik_solutions_within_bounds)} valid solutions.") - # TODO filter the IK solutions -> warn when nothing is left if self.filter_goal_configurations_fn is not None: ik_solutions_filtered = self.filter_goal_configurations_fn(ik_solutions_valid) if len(ik_solutions_filtered) == 0: diff --git a/notebooks/03_dual_arm_planning.ipynb b/notebooks/03_dual_arm_planning.ipynb index 018e644..1f2a885 100644 --- a/notebooks/03_dual_arm_planning.ipynb +++ b/notebooks/03_dual_arm_planning.ipynb @@ -287,9 +287,20 @@ "metadata": {}, "outputs": [], "source": [ - "from airo_drake import time_parametrize_toppra\n", + "from airo_drake import time_parametrize_toppra, animate_dual_joint_trajectory\n", "\n", - "trajectory = time_parametrize_toppra(scene.robot_diagram.plant(), path)" + "trajectory = time_parametrize_toppra(scene.robot_diagram.plant(), path)\n", + "\n", + "animate_dual_joint_trajectory(scene.meshcat, scene.robot_diagram, scene.arm_left_index, scene.arm_right_index, trajectory)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### 2.2 Joints to single arm joints (🦾,🦾) -> (🦾,💤)\n", + "\n", + "Note that you are allow to set one of the goals to `None` to indicate that that arm should not move." ] }, { @@ -298,8 +309,16 @@ "metadata": {}, "outputs": [], "source": [ - "from airo_drake import animate_dual_joint_trajectory\n", - "\n", + "path = planner.plan_to_joint_configuration(tangled_joints_left, tangled_joints_right, home_joints_left, None)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "trajectory = time_parametrize_toppra(scene.robot_diagram.plant(), path)\n", "animate_dual_joint_trajectory(scene.meshcat, scene.robot_diagram, scene.arm_left_index, scene.arm_right_index, trajectory)" ] }, @@ -307,9 +326,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "### 2.2 Joints to single arm joints (🦾,🦾) -> (🦾,💤)\n", - "\n", - "Note that you are allow to set one of the goals to `None` to indicate that that arm should not move." + "### 2.3 Joints to single TCP pose (🦾,🦾) -> (💤,📐)" ] }, { @@ -318,7 +335,14 @@ "metadata": {}, "outputs": [], "source": [ - "path = planner.plan_to_joint_configuration(tangled_joints_left, tangled_joints_right, home_joints_left, None)" + "from airo_drake import animate_joint_configurations\n", + "\n", + "grasp_pose = np.identity(4)\n", + "grasp_pose[2, 3] = 0.4\n", + "\n", + "grasp_joints = inverse_kinematics_right_fn(grasp_pose)\n", + "\n", + "animate_joint_configurations(scene.meshcat, scene.robot_diagram, scene.arm_right_index, grasp_joints, context=context)" ] }, { @@ -327,8 +351,7 @@ "metadata": {}, "outputs": [], "source": [ - "trajectory = time_parametrize_toppra(scene.robot_diagram.plant(), path)\n", - "animate_dual_joint_trajectory(scene.meshcat, scene.robot_diagram, scene.arm_left_index, scene.arm_right_index, trajectory)" + "path = planner.plan_to_tcp_pose(tangled_joints_left, tangled_joints_right, None, grasp_pose)" ] }, {