From ff488465f0bb1e6e5133f737b9dd17978ed7e57d Mon Sep 17 00:00:00 2001 From: Mathieu De Coster Date: Thu, 1 Aug 2024 10:13:58 +0200 Subject: [PATCH] Add pip install note for ubuntu 22.04+ to README --- README.md | 5 + airo_planner/interfaces.py | 53 +- .../ompl/mobile_platform_ompl_planner.py | 44 + notebooks/04_mobile_platform_planning.ipynb | 942 ++++++++++++++++++ pyproject.toml | 0 5 files changed, 1031 insertions(+), 13 deletions(-) mode change 100644 => 100755 airo_planner/interfaces.py create mode 100755 airo_planner/ompl/mobile_platform_ompl_planner.py create mode 100755 notebooks/04_mobile_platform_planning.ipynb mode change 100644 => 100755 pyproject.toml diff --git a/README.md b/README.md index 0429df0..15d2834 100644 --- a/README.md +++ b/README.md @@ -49,6 +49,11 @@ We depend on `ompl` with its Python bindings, which are not available on PyPI ye pip install https://github.com/ompl/ompl/releases/download/prerelease/ompl-1.6.0-cp310-cp310-manylinux_2_28_x86_64.whl ``` +For Ubuntu 22.04, use a different wheel: +``` +pip install https://github.com/ompl/ompl/releases/download/prerelease/ompl-1.6.0-cp311-cp311-manylinux_2_28_x86_64.whl +``` + ## Developer guide 🛠️ See the [`airo-mono`](https://github.com/airo-ugent/airo-mono) developer guide. A very similar process and tools are used for this package. diff --git a/airo_planner/interfaces.py b/airo_planner/interfaces.py old mode 100644 new mode 100755 index 8f9538e..f748330 --- a/airo_planner/interfaces.py +++ b/airo_planner/interfaces.py @@ -1,6 +1,33 @@ import abc -from airo_typing import HomogeneousMatrixType, JointConfigurationType, JointPathType +from airo_typing import HomogeneousMatrixType, JointConfigurationType, JointPathType, Vector3DType + + +# TODO: plan based on wheel configs as well or instead? +class MobilePlatformPlanner(abc.ABC): + """Base class that defines an interface for mobile platform motion planners, + where the platform is represented as an unbounded planar joint. + + The idea is that the custom settings for each motion planner are provided + through the constructor. After creation all motion planners can then be + used in the same way, e.g. for benchmarking. + """ + + @abc.abstractmethod + # TODO: Attitude2DType? + def plan_to_pose(self, start_pose: Vector3DType, goal_pose: Vector3DType) -> JointPathType: + """Plan a path from a start pose to a goal pose. + + Args: + start_pose: The (x, y, theta) start pose. + goal_pose: The (x, y, theta) goal pose. + + Returns: + A discretized path from the start pose to the goal pose. + + Raises: + NoPathFoundError: If no path could be found between the start and + goal configuration.""" class SingleArmPlanner(abc.ABC): @@ -12,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. @@ -35,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).") @@ -49,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. @@ -83,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 new file mode 100755 index 0000000..2a3c764 --- /dev/null +++ b/airo_planner/ompl/mobile_platform_ompl_planner.py @@ -0,0 +1,44 @@ +import numpy as np +from airo_typing import Vector3DType, JointPathType, JointConfigurationCheckerType +from loguru import logger + +from airo_planner import create_simple_setup, state_to_ompl, NoPathFoundError, solve_and_smooth_path +from airo_planner.interfaces import MobilePlatformPlanner + + +class MobilePlatformOmplPlanner(MobilePlatformPlanner): + """Utility class for mobile platform motion planning using OMPL. + + The purpose of this class is to make working with OMPL easier. It basically + just handles the creation of OMPL objects and the conversion between numpy + arrays and OMPL states and paths. After creating an instance of this class, + you can also extract the SimpleSetup object and use it directly if you want. + This can be useful for benchmarking with the OMPL benchmarking tools. + """ + + def __init__(self, is_state_valid_fn: JointConfigurationCheckerType): + # TODO: JointConfigurationCheckerType is not ideal here, should be a new type. + self.is_state_valid_fn = is_state_valid_fn + + joint_bounds = ( + np.full(3, -np.inf), + np.full(3, np.inf), + ) + 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: + self._simple_setup.clear() + + space = self._simple_setup.getStateSpace() + start_state = state_to_ompl(start_pose, space) + goal_state = state_to_ompl(goal_pose, space) + self._simple_setup.setStartAndGoalStates(start_state, goal_state) + + path = solve_and_smooth_path(self._simple_setup) + + if path is None: + raise NoPathFoundError(start_pose, goal_pose) + + logger.success(f"Successfully found path (with {len(path)} waypoints).") + + return path diff --git a/notebooks/04_mobile_platform_planning.ipynb b/notebooks/04_mobile_platform_planning.ipynb new file mode 100755 index 0000000..a0e4e3b --- /dev/null +++ b/notebooks/04_mobile_platform_planning.ipynb @@ -0,0 +1,942 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Mobile platform planning 🏎️" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## 1. Setup 🏗️️\n", + "\n", + "### 1.1 Building our mobile platform scene" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "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=, mobile_platform_index=ModelInstanceIndex(2), arm_index=ModelInstanceIndex(12), gripper_index=ModelInstanceIndex(13), meshcat=)" + ] + }, + "execution_count": 1, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "import numpy as np\n", + "from pydrake.planning import RobotDiagramBuilder, SceneGraphCollisionChecker\n", + "from pydrake.math import RigidTransform, RollPitchYaw\n", + "from airo_drake import MobilePlatformWithSingleArmScene, add_mobile_platform, finish_build, add_meshcat, add_manipulator, attach_mobile_platform_to_world\n", + "\n", + "robot_diagram_builder = RobotDiagramBuilder()\n", + "\n", + "meshcat = add_meshcat(robot_diagram_builder)\n", + "mobi_index = add_mobile_platform(robot_diagram_builder, drive_positions=(np.array([1, -0.5]), np.array([1, 0.5]), np.array([-1, -0.5]),\n", + " np.array([-1, 0.5])), cpu_position=np.array([0, -0.5]), battery_position=np.array([0, 0.5]))\n", + "mobi_frame = robot_diagram_builder.plant().GetFrameByName(\"base_link\", mobi_index)\n", + "\n", + "# For these two values, see the API of `add_mobile_platform`.\n", + "side_height = 0.43\n", + "roof_thickness = 0.03\n", + "arm_transform = RigidTransform(p=[0.2445, 0, side_height + roof_thickness], rpy=RollPitchYaw([0, 0, -np.pi / 2]))\n", + "\n", + "arm_index, gripper_index = add_manipulator(robot_diagram_builder, \"ur5e\", \"robotiq_2f_85\", arm_transform, static_gripper=True, parent_frame=mobi_frame)\n", + "\n", + "attach_mobile_platform_to_world(robot_diagram_builder, mobi_index)\n", + "\n", + "robot_diagram, context = finish_build(robot_diagram_builder)\n", + "\n", + "scene = MobilePlatformWithSingleArmScene(robot_diagram, mobi_index, arm_index, gripper_index, meshcat)\n", + "scene" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### 1.2 Mobile platform `SceneGraphCollisionChecker` 💥\n", + "\n", + "We need to provide the model indices for the mobile robot. We only motion plan the mobile platform for now, not everything at once." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "INFO:drake:Allocating contexts to support implicit context parallelism 8\n" + ] + } + ], + "source": [ + "collision_checker = SceneGraphCollisionChecker(\n", + " model=scene.robot_diagram,\n", + " robot_model_instances=[\n", + " scene.mobile_platform_index,\n", + " ],\n", + " edge_step_size=0.125, # Arbitrary value: we don't use the CheckEdgeCollisionFree\n", + " env_collision_padding=0.005,\n", + " self_collision_padding=0.005,\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "9" + ] + }, + "execution_count": 3, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "scene.robot_diagram.plant().num_positions()" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "True" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "collision_checker.CheckConfigCollisionFree(np.zeros(9))" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "True" + ] + }, + "execution_count": 5, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "home_joints = np.deg2rad([0, 0, 0, 180, -120, 60, -30, -90, -90])\n", + "\n", + "collision_checker.CheckConfigCollisionFree(home_joints)" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [], + "source": [ + "plant = scene.robot_diagram.plant()\n", + "plant_context = plant.GetMyContextFromRoot(context)\n", + "\n", + "plant.SetPositions(plant_context, home_joints)\n", + "scene.robot_diagram.ForcedPublish(context) # updates the meshcat visualization" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### 1.3 Inverse kinematics for the mobile robot 🧮\n", + "\n", + "The mobile robot's 3 DOF are represented in Drake with a `PlanarJoint` with no bounds on the `x`, `y` and `theta` values.\n", + "This means that the forward and inverse kinematics of the platform can be implemented with identity functions.\n", + "\n", + "In other words, we don't even need to define an IK function!" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### 1.4 `MobileRobotOmplPlanner` 🧭" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "ename": "ModuleNotFoundError", + "evalue": "No module named 'ompl'", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mModuleNotFoundError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[7], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mairo_planner\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m MobileRobotOmplPlanner\n\u001b[1;32m 3\u001b[0m planner \u001b[38;5;241m=\u001b[39m MobileRobotOmplPlanner(\n\u001b[1;32m 4\u001b[0m is_state_valid_fn\u001b[38;5;241m=\u001b[39mcollision_checker\u001b[38;5;241m.\u001b[39mCheckConfigCollisionFree\n\u001b[1;32m 5\u001b[0m )\n", + "File \u001b[0;32m~/Documents/postdoc/airo-planner/airo_planner/__init__.py:37\u001b[0m\n\u001b[1;32m 31\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mairo_planner\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mselection\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mgoal_selection\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m (\n\u001b[1;32m 32\u001b[0m filter_with_distance_to_configurations,\n\u001b[1;32m 33\u001b[0m rank_by_distance_to_desirable_configurations,\n\u001b[1;32m 34\u001b[0m )\n\u001b[1;32m 35\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mairo_planner\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mselection\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mpath_selection\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m choose_shortest_path\n\u001b[0;32m---> 37\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mairo_planner\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mompl\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mutilities\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m (\n\u001b[1;32m 38\u001b[0m function_to_ompl,\n\u001b[1;32m 39\u001b[0m state_to_ompl,\n\u001b[1;32m 40\u001b[0m path_from_ompl,\n\u001b[1;32m 41\u001b[0m state_from_ompl,\n\u001b[1;32m 42\u001b[0m bounds_to_ompl,\n\u001b[1;32m 43\u001b[0m create_simple_setup,\n\u001b[1;32m 44\u001b[0m solve_and_smooth_path,\n\u001b[1;32m 45\u001b[0m )\n\u001b[1;32m 47\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mairo_planner\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mmultiple_goal_planner\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m (\n\u001b[1;32m 48\u001b[0m MultipleGoalPlanner,\n\u001b[1;32m 49\u001b[0m JointConfigurationsModifierType,\n\u001b[1;32m 50\u001b[0m JointPathChooserType,\n\u001b[1;32m 51\u001b[0m )\n\u001b[1;32m 53\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mairo_planner\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mfunction_conversions\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m (\n\u001b[1;32m 54\u001b[0m convert_dual_arm_joints_modifier_to_single_arm,\n\u001b[1;32m 55\u001b[0m convert_dual_arm_path_chooser_to_single_arm,\n\u001b[1;32m 56\u001b[0m )\n", + "File \u001b[0;32m~/Documents/postdoc/airo-planner/airo_planner/ompl/utilities.py:5\u001b[0m\n\u001b[1;32m 3\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mnumpy\u001b[39;00m \u001b[38;5;28;01mas\u001b[39;00m \u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mairo_typing\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m JointConfigurationCheckerType, JointPathType\n\u001b[0;32m----> 5\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mompl\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m base \u001b[38;5;28;01mas\u001b[39;00m ob\n\u001b[1;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mompl\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m geometric \u001b[38;5;28;01mas\u001b[39;00m og\n\u001b[1;32m 8\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mairo_planner\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m JointBoundsType\n", + "\u001b[0;31mModuleNotFoundError\u001b[0m: No module named 'ompl'" + ] + } + ], + "source": [ + "from airo_planner import MobileRobotOmplPlanner\n", + "\n", + "planner = MobileRobotOmplPlanner(\n", + " is_state_valid_fn=collision_checker.CheckConfigCollisionFree\n", + ")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We use the terms \"left\" and \"right\" for the two arms, note that you can choose yourself which arm is which, however you must take care to be consistent which arms configuration is first and which is second in the `is_state_valid_fn`. We recommend for simplificity to choose as left arm the arm that also appear on the left side of image from the robot's primary camera, as this is how an egocentric robot would name its arms.\n", + "\n", + "The API for dual arm motion planning is very similar to the single arm API, expect that you can supply two joint configurations or two tcp poses instead of one. " + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## 2. Scenarios 🖼️\n", + "\n", + "In this section we demonstrate the `airo-planner` API by going over the main Cloth Competition use cases.\n", + "\n", + "TODO:\n", + "- scene without cloth obstacle go home\n", + "- scene grasp highest (plan to single TCP pose)\n", + "- scene with cloth obstacle grasp single arm and filter -> pregrasp no collision, grasp can collide\n", + "- scene go to stretch poses (plan to dual TCP poses)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### 2.1 Joints to joints (🦾,🦾) -> (🦾,🦾)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "tangled_joints_left = np.deg2rad([0, -90, -90, -90, 90, 0])\n", + "tangled_joints_right = np.deg2rad([-136, -116, -110, -133, 40, 0])\n", + "tangled_joints = np.concatenate([tangled_joints_left, tangled_joints_right])\n", + "\n", + "plant.SetPositions(plant_context, tangled_joints)\n", + "scene.robot_diagram.ForcedPublish(context) # updates the meshcat visualization" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "path = planner.plan_to_joint_configuration(\n", + " tangled_joints_left, tangled_joints_right, home_joints_left, home_joints_right\n", + ")\n", + "print(path.shape)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from airo_drake import time_parametrize_toppra, animate_dual_joint_trajectory\n", + "\n", + "trajectory = time_parametrize_toppra(scene.robot_diagram.plant(), path)\n", + "\n", + "animate_dual_joint_trajectory(\n", + " scene.meshcat, scene.robot_diagram, scene.arm_left_index, scene.arm_right_index, trajectory\n", + ")" + ] + }, + { + "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." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "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(\n", + " scene.meshcat, scene.robot_diagram, scene.arm_left_index, scene.arm_right_index, trajectory\n", + ")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### 2.3 Joints to single TCP pose (🦾,🦾) -> (💤,📐)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from airo_drake import animate_joint_configurations, visualize_frame\n", + "\n", + "grasp_pose_hard = np.identity(4)\n", + "grasp_pose_hard[2, 3] = 0.4\n", + "\n", + "visualize_frame(scene.meshcat, \"grasp_pose\", grasp_pose_hard)\n", + "\n", + "grasp_joints = inverse_kinematics_right_fn(grasp_pose_hard)\n", + "\n", + "animate_joint_configurations(scene.meshcat, scene.robot_diagram, scene.arm_right_index, grasp_joints, context=context)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "path = planner.plan_to_tcp_pose(tangled_joints_left, home_joints_right, None, grasp_pose_hard)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "trajectory = time_parametrize_toppra(scene.robot_diagram.plant(), path)\n", + "animate_dual_joint_trajectory(\n", + " scene.meshcat, scene.robot_diagram, scene.arm_left_index, scene.arm_right_index, trajectory\n", + ")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### 2.3 Joints to dual TCP poses (🦾,🦾) -> (📐,📐)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from airo_typing import RotationMatrixType\n", + "\n", + "\n", + "def hang_in_the_air_tcp_orientation(left: bool) -> RotationMatrixType:\n", + " gripper_forward_direction = np.array([0, -1, 0]) if left else np.array([0, 1, 0])\n", + " Z = gripper_forward_direction / np.linalg.norm(gripper_forward_direction)\n", + " X = np.array([0, 0, 1]) if left else np.array([0, 0, -1])\n", + " Y = np.cross(Z, X)\n", + " return np.column_stack([X, Y, Z])\n", + "\n", + "\n", + "def hang_in_the_air_tcp_pose(left: bool) -> HomogeneousMatrixType:\n", + " position = np.array([0, 0, 0.9]) # 1 m is too close to a singularity\n", + " gripper_orientation = hang_in_the_air_tcp_orientation(left)\n", + "\n", + " gripper_pose = np.identity(4)\n", + " gripper_pose[0:3, 0:3] = gripper_orientation\n", + " gripper_pose[0:3, 3] = position\n", + " return gripper_pose\n", + "\n", + "\n", + "hang_pose_left = hang_in_the_air_tcp_pose(left=True)\n", + "hang_pose_right = hang_in_the_air_tcp_pose(left=False)\n", + "\n", + "stretch_pose_left = hang_pose_left.copy()\n", + "stretch_pose_right = hang_pose_right.copy()\n", + "\n", + "stretch_pose_left[:3, 3] += np.array([-0.4, 0.02, 0])\n", + "stretch_pose_right[:3, 3] += np.array([-0.4, -0.02, 0])\n", + "\n", + "visualize_frame(scene.meshcat, \"stretch_pose_left\", stretch_pose_left, opacity=0.5)\n", + "visualize_frame(scene.meshcat, \"stretch_pose_right\", stretch_pose_right, opacity=0.5)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "path = planner.plan_to_tcp_pose(tangled_joints_left, tangled_joints_right, stretch_pose_left, stretch_pose_right)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "trajectory = time_parametrize_toppra(scene.robot_diagram.plant(), path)\n", + "animate_dual_joint_trajectory(\n", + " scene.meshcat, scene.robot_diagram, scene.arm_left_index, scene.arm_right_index, trajectory\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "scene.meshcat.Delete(\"stretch_pose_left\")\n", + "scene.meshcat.Delete(\"stretch_pose_right\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### 2.4 Joints to single TCP pose with ranking (🦾,🦾) -> (💤,📐)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from airo_planner import rank_by_distance_to_desirable_configurations\n", + "from airo_planner import stack_joints\n", + "\n", + "# Note how we set desirable left joints to it's start configuration as the left arm is not involved in the task\n", + "desirable_configurations = [stack_joints(tangled_joints_left, home_joints_right)]\n", + "\n", + "rank_fn = partial(rank_by_distance_to_desirable_configurations, desirable_configurations=desirable_configurations)\n", + "\n", + "\n", + "planner.rank_goal_configurations_fn = rank_fn\n", + "path = planner.plan_to_tcp_pose(tangled_joints_left, tangled_joints_right, None, hang_pose_right)\n", + "planner.rank_goal_configurations_fn = None # Remove the rank function for when other cells are run" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "trajectory = time_parametrize_toppra(scene.robot_diagram.plant(), path)\n", + "animate_dual_joint_trajectory(\n", + " scene.meshcat, scene.robot_diagram, scene.arm_left_index, scene.arm_right_index, trajectory\n", + ")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## 3. Advanced filtering and ranking for grasping ⚙️\n", + "\n", + "In this example we show the full capability of airo-planner for a complex cloth competition use cases, grasping a hanging cloth with one arm that is held in the air with another arm.\n", + "\n", + "We want to approach the grasp pose linearly from pregrasp pose that lies a few cm behind it (several distances can be tried). The path to the pregrasp pose should not collide with the convex hull of the hanging cloth. The path from the pregrasp pose to the grasp pose should not collide with the environment, but is allowed to contact the cloth. \n", + "\n", + "Additionally, of all solution paths, we prefer paths where the linear grasp motion can be executed \"comfortably\", so we rank the pregrasp joints by the TOPP-RA calculated duration of the grasp trajectory.\n", + "\n", + "To summarize:\n", + "* We will choose a pregrasp distance and calculate the resulting pregrasp pose and grasp TCP path.\n", + "* We calculate the environment-collision-free joint paths that can execute this TCP path.\n", + "* We calculate their duration and rank the pregrasp joints according to grasp path duration\n", + "* We try to plan a cloth-collision-free pregrasp path to the pregrasp joints \n", + "* If anything fails, try a different pregrasp distance\n", + "* If all pregrasp distances fail, consider the grasp pos unreachable." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### 3.1 Filtering collision" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "robot_diagram_builder = RobotDiagramBuilder()\n", + "meshcat = add_meshcat(robot_diagram_builder)\n", + "meshcat.SetCameraPose([-1.5, 0, 1.0], [0, 0, 0])\n", + "\n", + "(arm_left_index, arm_right_index), (gripper_left_index, gripper_right_index) = add_cloth_competition_dual_ur5e_scene(\n", + " robot_diagram_builder, X_W_L, X_W_R, add_cylinder=True\n", + ")\n", + "robot_diagram, context = finish_build(robot_diagram_builder, meshcat)\n", + "\n", + "scene_with_cloth = DualArmScene(\n", + " robot_diagram,\n", + " arm_left_index,\n", + " arm_right_index,\n", + " gripper_left_index,\n", + " gripper_right_index,\n", + " meshcat,\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "hang_pose_left = hang_in_the_air_tcp_pose(left=True)\n", + "\n", + "hang_joints_left = inverse_kinematics_left_fn(hang_pose_left)\n", + "\n", + "for joints in hang_joints_left:\n", + " print(joints)\n", + "\n", + "\n", + "start_joints_left = rank_by_distance_to_desirable_configurations(hang_joints_left, [home_joints_left])[0]\n", + "\n", + "print(\"Chosen:\")\n", + "print(start_joints_left)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "start_joints_right = home_joints_right\n", + "start_joints = np.concatenate([start_joints_left, start_joints_right])\n", + "\n", + "plant = robot_diagram.plant()\n", + "plant_context = plant.GetMyContextFromRoot(context)\n", + "plant.SetPositions(plant_context, start_joints)\n", + "robot_diagram.ForcedPublish(context) # updates the meshcat visualization" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Visualization of plannig without the cloth obstacle" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "path = planner.plan_to_tcp_pose(start_joints_left, start_joints_right, None, grasp_pose_hard)\n", + "trajectory = time_parametrize_toppra(scene.robot_diagram.plant(), path)\n", + "animate_dual_joint_trajectory(\n", + " scene_with_cloth.meshcat,\n", + " scene_with_cloth.robot_diagram,\n", + " scene_with_cloth.arm_left_index,\n", + " scene_with_cloth.arm_right_index,\n", + " trajectory,\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "collision_checker_with_cloth = SceneGraphCollisionChecker(\n", + " model=scene_with_cloth.robot_diagram,\n", + " robot_model_instances=[\n", + " scene_with_cloth.arm_left_index,\n", + " scene_with_cloth.arm_right_index,\n", + " scene_with_cloth.gripper_left_index,\n", + " scene_with_cloth.gripper_right_index,\n", + " ],\n", + " edge_step_size=0.125, # Arbitrary value: we don't use the CheckEdgeCollisionFree\n", + " env_collision_padding=0.005,\n", + " self_collision_padding=0.005,\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "collision_checker_with_cloth.CheckConfigCollisionFree(trajectory.value(trajectory.end_time()))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "is_state_valid_fn_pregrasp = collision_checker_with_cloth.CheckConfigCollisionFree\n", + "is_state_valid_fn_grasp = collision_checker.CheckConfigCollisionFree" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "planner_pregrasp = DualArmOmplPlanner(\n", + " is_state_valid_fn=is_state_valid_fn_pregrasp,\n", + " inverse_kinematics_left_fn=inverse_kinematics_left_fn,\n", + " inverse_kinematics_right_fn=inverse_kinematics_right_fn,\n", + " joint_bounds_left=joint_bounds,\n", + " joint_bounds_right=joint_bounds,\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from airo_typing import InverseKinematicsFunctionType, JointConfigurationCheckerType, JointPathType\n", + "from loguru import logger\n", + "from pydrake.trajectories import PiecewisePose, Trajectory\n", + "from pydrake.math import RigidTransform\n", + "from airo_drake import discretize_drake_pose_trajectory\n", + "from airo_drake import calculate_valid_joint_paths\n", + "from functools import partial\n", + "from airo_planner import filter_with_distance_to_configurations, PlannerError\n", + "from pydrake.multibody.plant import MultibodyPlant\n", + "from airo_drake import concatenate_drake_trajectories\n", + "\n", + "\n", + "# TODO consider making a class for this that stores debug information\n", + "def plan_pregrasp_and_grasp_trajetory(\n", + " planner_pregrasp: DualArmOmplPlanner,\n", + " grasp_pose: HomogeneousMatrixType,\n", + " start_configuration_left: JointConfigurationType,\n", + " start_configuration_right: JointConfigurationType,\n", + " inverse_kinematics_left_fn: InverseKinematicsFunctionType, # same comment as for is_state_valid_fn_grasp\n", + " inverse_kinematics_right_fn: InverseKinematicsFunctionType,\n", + " is_state_valid_fn_grasp: JointConfigurationCheckerType, # could make this optional and use planner's by default\n", + " plant_toppra: MultibodyPlant,\n", + " with_left: bool = True,\n", + ") -> tuple[Trajectory]:\n", + "\n", + " # We add 1.0 so at least one pregrasp distance fails:\n", + " pregrasp_distances_to_try = [0.05, 0.1, 0.15] # , 0.2, 0.25]\n", + "\n", + " # is_state_valid_fn_grasp currently still takes a 12-DoF configuration\n", + " def is_single_arm_state_valid_fn_grasp(joint_configuration: JointConfigurationType) -> bool:\n", + " if with_left:\n", + " return is_state_valid_fn_grasp(stack_joints(joint_configuration, start_configuration_right))\n", + " else:\n", + " return is_state_valid_fn_grasp(stack_joints(start_configuration_left, joint_configuration))\n", + "\n", + " def hardcoded_cost_fn(\n", + " joint_configuration: JointConfigurationType,\n", + " known_joint_configurations: list[JointConfigurationType],\n", + " costs: list[float],\n", + " ) -> float:\n", + " distances = [\n", + " np.linalg.norm(joint_configuration - known_configuration)\n", + " for known_configuration in known_joint_configurations\n", + " ]\n", + " if np.min(distances) > 0.001:\n", + " logger.warning(f\"Joint configuration is not close to any known configurations. {joint_configuration} \")\n", + " return costs[np.argmin(distances)]\n", + "\n", + " def rank_with_cost_fn(\n", + " joint_configurations: list[JointConfigurationType], cost_fn: JointConfigurationCheckerType\n", + " ) -> list[JointConfigurationType]:\n", + " return sorted(joint_configurations, key=cost_fn)\n", + "\n", + " for distance in pregrasp_distances_to_try:\n", + " logger.info(f\"Planning to pregrasp pose at distance {distance}.\")\n", + " # 1. Compute pregrasp pose\n", + " pregrasp_pose = grasp_pose.copy()\n", + " pregrasp_pose[0:3, 3] -= distance * pregrasp_pose[0:3, 2]\n", + "\n", + " pregrasp_pose_left = pregrasp_pose if with_left else None\n", + " pregrasp_pose_right = pregrasp_pose if not with_left else None\n", + "\n", + " # 2. Compute grasp TCP path\n", + " rigid_transforms = [RigidTransform(pose) for pose in [pregrasp_pose, grasp_pose]]\n", + " times = np.linspace(0, 1, len(rigid_transforms))\n", + " pose_trajectory = PiecewisePose.MakeLinear(times=times, poses=rigid_transforms)\n", + " grasp_tcp_path = discretize_drake_pose_trajectory(pose_trajectory).poses\n", + "\n", + " # 3 Compute valid grasp joint paths\n", + " inverse_kinematics_fn = inverse_kinematics_left_fn if with_left else inverse_kinematics_right_fn\n", + "\n", + " grasp_path_single_arm = calculate_valid_joint_paths(\n", + " grasp_tcp_path, inverse_kinematics_fn, is_single_arm_state_valid_fn_grasp\n", + " )\n", + "\n", + " if len(grasp_path_single_arm) == 0:\n", + " logger.info(f\"No valid grasp joint paths found for distance {distance}, continuing to next distance.\")\n", + " continue\n", + "\n", + " if with_left:\n", + " grasp_paths = [stack_joints(path, start_configuration_right) for path in grasp_path_single_arm]\n", + " else:\n", + " grasp_paths = [stack_joints(start_configuration_left, path) for path in grasp_path_single_arm]\n", + "\n", + " # Create filter function\n", + " grasp_path_starts = [path[0] for path in grasp_paths]\n", + " filter_fn = partial(filter_with_distance_to_configurations, joint_configurations_close=grasp_path_starts)\n", + "\n", + " # Create rank function\n", + " grasp_trajectories = []\n", + " times = []\n", + " for path in grasp_paths:\n", + " trajectory = time_parametrize_toppra(plant_toppra, path)\n", + " times.append(trajectory.end_time())\n", + " grasp_trajectories.append(trajectory)\n", + "\n", + " cost_fn = partial(hardcoded_cost_fn, known_joint_configurations=grasp_path_starts, costs=times)\n", + " rank_fn = partial(rank_with_cost_fn, cost_fn=cost_fn)\n", + "\n", + " # Plan\n", + " planner_pregrasp.filter_goal_configurations_fn = filter_fn\n", + " planner_pregrasp.rank_goal_configurations_fn = rank_fn\n", + "\n", + " try:\n", + " pregrasp_path = planner_pregrasp.plan_to_tcp_pose(\n", + " start_configuration_left, start_configuration_right, pregrasp_pose_left, pregrasp_pose_right\n", + " )\n", + " except PlannerError as e:\n", + " logger.info(\n", + " f\"Failed to plan to pregrasp pose at distance {distance}, continuing to next distance. Exception was:\\n {e}.\"\n", + " )\n", + " continue\n", + "\n", + " pregrasp_trajectory = time_parametrize_toppra(plant_toppra, pregrasp_path)\n", + "\n", + " # # Find the grasp trajectory of which the start is closest to the pregrasp path end (=pregrasp end joints)\n", + " # We will likely want an airo-planner helper function for this\n", + " pregrasp_end_joints = pregrasp_path[-1]\n", + " distances = [np.linalg.norm(start - pregrasp_end_joints) for start in grasp_path_starts]\n", + " index_of_closest_start = np.argmin(distances)\n", + "\n", + " assert np.isclose(distances[index_of_closest_start], 0, atol=0.01) # sanity check\n", + "\n", + " grasp_trajectory = grasp_trajectories[index_of_closest_start]\n", + "\n", + " # final set: concatenate pregrasp and grasp trajectories\n", + " pregrasp_and_grasp_trajectory = concatenate_drake_trajectories([pregrasp_trajectory, grasp_trajectory])\n", + "\n", + " return pregrasp_and_grasp_trajectory\n", + "\n", + " raise RuntimeError(\"Grasp planner exhausted all pregrasp poses to try\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from airo_drake import visualize_frame\n", + "\n", + "grasp_location = np.array([0.0, 0.0, 0.4])\n", + "\n", + "gripper_forward_direction = np.array([1, 0, 0])\n", + "\n", + "Z = gripper_forward_direction / np.linalg.norm(gripper_forward_direction)\n", + "Y = np.array([0, 0, -1]) # 0, 0, 1 is also an option\n", + "X = np.cross(Y, Z)\n", + "\n", + "grasp_orientation = np.column_stack([X, Y, Z])\n", + "grasp_pose_easy = np.identity(4)\n", + "grasp_pose_easy[0:3, 0:3] = grasp_orientation\n", + "grasp_pose_easy[0:3, 3] = grasp_location\n", + "\n", + "visualize_frame(scene_with_cloth.meshcat, \"grasp_pose_easy\", grasp_pose_easy, length=0.25)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "pregrasp_and_grasp_trajectory_easy = plan_pregrasp_and_grasp_trajetory(\n", + " planner_pregrasp,\n", + " grasp_pose_easy,\n", + " start_joints_left,\n", + " start_joints_right,\n", + " inverse_kinematics_left_fn,\n", + " inverse_kinematics_right_fn,\n", + " is_state_valid_fn_grasp,\n", + " plant,\n", + " with_left=False,\n", + ")\n", + "\n", + "animate_dual_joint_trajectory(\n", + " scene_with_cloth.meshcat,\n", + " scene_with_cloth.robot_diagram,\n", + " scene_with_cloth.arm_left_index,\n", + " scene_with_cloth.arm_right_index,\n", + " pregrasp_and_grasp_trajectory_easy,\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "scene_with_cloth.meshcat.Delete(\"grasp_pose_easy\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "grasp_location = np.array([-0.08, 0.0, 0.33])\n", + "\n", + "gripper_forward_direction = np.array([1, -1, 0])\n", + "\n", + "Z = gripper_forward_direction / np.linalg.norm(gripper_forward_direction)\n", + "Y = np.array([0, 0, -1]) # 0, 0, 1 is also an option\n", + "X = np.cross(Y, Z)\n", + "\n", + "grasp_orientation = np.column_stack([X, Y, Z])\n", + "grasp_pose_hard = np.identity(4)\n", + "grasp_pose_hard[0:3, 0:3] = grasp_orientation\n", + "grasp_pose_hard[0:3, 3] = grasp_location\n", + "\n", + "visualize_frame(scene_with_cloth.meshcat, \"grasp_pose_hard\", grasp_pose_hard, length=0.25)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "pregrasp_and_grasp_trajectory_hard = plan_pregrasp_and_grasp_trajetory(\n", + " planner_pregrasp,\n", + " grasp_pose_hard,\n", + " start_joints_left,\n", + " start_joints_right,\n", + " inverse_kinematics_left_fn,\n", + " inverse_kinematics_right_fn,\n", + " is_state_valid_fn_grasp,\n", + " plant,\n", + " with_left=False,\n", + ")\n", + "\n", + "animate_dual_joint_trajectory(\n", + " scene_with_cloth.meshcat,\n", + " scene_with_cloth.robot_diagram,\n", + " scene_with_cloth.arm_left_index,\n", + " scene_with_cloth.arm_right_index,\n", + " pregrasp_and_grasp_trajectory_hard,\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "scene_with_cloth.meshcat.Delete(\"grasp_pose_hard\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.10.12" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/pyproject.toml b/pyproject.toml old mode 100644 new mode 100755