diff --git a/airo_drake/__init__.py b/airo_drake/__init__.py index 0a70057..0ac4cce 100644 --- a/airo_drake/__init__.py +++ b/airo_drake/__init__.py @@ -5,7 +5,7 @@ # you won't be able to import the dummy module. from airo_drake.building.finish import finish_build from airo_drake.building.floor import add_floor -from airo_drake.building.manipulator import add_manipulator +from airo_drake.building.manipulator import X_URBASE_ROSBASE, X_URTOOL0_ROBOTIQ, add_manipulator from airo_drake.building.meshcat import add_meshcat from airo_drake.scene import DualArmScene, SingleArmScene from airo_drake.time_parametrization.toppra import time_parametrize_toppra @@ -16,6 +16,8 @@ __all__ = [ "add_floor", "add_manipulator", + "X_URBASE_ROSBASE", + "X_URTOOL0_ROBOTIQ", "add_meshcat", "finish_build", "SingleArmScene", diff --git a/notebooks/03_collision_checking.ipynb b/notebooks/03_collision_checking.ipynb new file mode 100644 index 0000000..f21b6a9 --- /dev/null +++ b/notebooks/03_collision_checking.ipynb @@ -0,0 +1,235 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Collision checking tutorial 💥\n", + "\n", + "This notebook shows how to:\n", + "* Set up collision checking for joint configurations\n", + "* Apply collision checking to joint and TCP paths" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## 1. Creating a `SceneGraphCollisionChecker` 🛡️\n", + "Drake provides a very convenient class called [`SceneGraphCollisionChecker`](https://drake.mit.edu/doxygen_cxx/group__planning__collision__checker.html) that checks for collisions between the robot's links itself and its environment.\n", + "After you set it up with a `RobotDiagram`, you can check for collisions with:\n", + "```python\n", + "collision_checker.CheckConfigCollisionFree(q)\n", + "```\n", + "Where `q` are all the joint positions of the `MultibodyPlant`.\n", + "\n", + "In the previous notebooks we showed how to quickly create a `RobotDiagram` with a UR5e and Robotiq 2F-85 gripper.\n", + "You might have noticed that gripper introduces 6 degrees of freedom.\n", + "However, in this notebook we will, for simplicity, make the gripper static and do collision checks with the gripper in a fixed open position." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "from pydrake.planning import RobotDiagramBuilder\n", + "from airo_drake import add_floor, add_meshcat, finish_build, X_URTOOL0_ROBOTIQ, X_URBASE_ROSBASE\n", + "from airo_drake import SingleArmScene\n", + "from pydrake.math import RigidTransform\n", + "import airo_models\n", + "\n", + "robot_diagram_builder = RobotDiagramBuilder()\n", + "\n", + "meshcat = add_meshcat(robot_diagram_builder)\n", + "add_floor(robot_diagram_builder)\n", + "\n", + "plant = robot_diagram_builder.plant()\n", + "parser = robot_diagram_builder.parser()\n", + "parser.SetAutoRenaming(True)\n", + "\n", + "# Load URDF files\n", + "arm_name = \"ur5e\"\n", + "gripper_name = \"robotiq_2f_85\"\n", + "arm_urdf_path = airo_models.get_urdf_path(arm_name)\n", + "gripper_urdf_path = airo_models.get_urdf_path(gripper_name)\n", + "\n", + "# Make the gripper static\n", + "gripper_urdf = airo_models.urdf.read_urdf(gripper_urdf_path)\n", + "airo_models.urdf.replace_value(gripper_urdf, \"@type\", \"revolute\", \"fixed\")\n", + "airo_models.urdf.delete_key(gripper_urdf, \"mimic\")\n", + "airo_models.urdf.delete_key(gripper_urdf, \"transmission\")\n", + "gripper_static_urdf_path = airo_models.urdf.write_urdf_to_tempfile(\n", + " gripper_urdf, gripper_urdf_path, prefix=f\"{gripper_name}_static_\"\n", + ")\n", + "\n", + "# Use static gripper\n", + "arm_index = parser.AddModels(arm_urdf_path)[0]\n", + "gripper_index = parser.AddModels(gripper_static_urdf_path)[0]\n", + "\n", + "# Weld some frames together\n", + "world_frame = plant.world_frame()\n", + "arm_frame = plant.GetFrameByName(\"base_link\", arm_index)\n", + "arm_tool_frame = plant.GetFrameByName(\"tool0\", arm_index)\n", + "gripper_frame = plant.GetFrameByName(\"base_link\", gripper_index)\n", + "\n", + "arm_rigid_transform = X_URBASE_ROSBASE\n", + "gripper_rigid_transform = X_URTOOL0_ROBOTIQ\n", + "\n", + "plant.WeldFrames(world_frame, arm_frame, arm_rigid_transform)\n", + "plant.WeldFrames(arm_tool_frame, gripper_frame, gripper_rigid_transform)\n", + "\n", + "robot_diagram, context = finish_build(robot_diagram_builder, meshcat)\n", + "del robot_diagram_builder # no longer needed\n", + "\n", + "scene = SingleArmScene(robot_diagram, arm_index, gripper_index, meshcat)\n", + "scene" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "plant.num_positions(), plant.num_positions(arm_index), plant.num_positions(gripper_index)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from pydrake.planning import SceneGraphCollisionChecker\n", + "\n", + "collision_checker = SceneGraphCollisionChecker(\n", + " model=scene.robot_diagram,\n", + " robot_model_instances=[scene.arm_index, scene.gripper_index],\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": "markdown", + "metadata": {}, + "source": [ + "## 2. Checking joint configurations 🤖\n", + "\n", + "Lets test some joint configurations for collisions!\n", + "\n", + "First, the default configuration. You can see in Meshcat that the robot is just barely not colliding with the table." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "collision_checker.CheckConfigCollisionFree(np.zeros(6))" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "UR robots are always in self-collsion when elbow joint is at 180 degrees:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "joints_self_collision = np.deg2rad([0, 0, 180, 0, 0, 0])\n", + "collision_checker.CheckConfigCollisionFree(joints_self_collision)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Lets double-check that visually:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "plant = scene.robot_diagram.plant()\n", + "plant_context = plant.GetMyContextFromRoot(context)\n", + "\n", + "plant.SetPositions(plant_context, scene.arm_index, joints_self_collision)\n", + "scene.robot_diagram.ForcedPublish(context) # updates the meshcat visualization" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We can make the robot collide with the table by make the shoulder angle slightly larger" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "joints_table_collision = np.deg2rad([0, 5, 0, 0, 0, 0])\n", + "collision_checker.CheckConfigCollisionFree(joints_table_collision)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "plant.SetPositions(plant_context, scene.arm_index, joints_table_collision)\n", + "scene.robot_diagram.ForcedPublish(context)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Checking joint paths 🛤️" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "cloth-competition", + "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.13" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +}