start for collision checking notebook

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",

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
+}