tutorials/hydroelastic_contact_nonconvex_mesh.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Hydroelastic Contact: Nonconvex Mesh\n",
    "For instructions on how to run these tutorial notebooks, please see the [index](./index.ipynb).\n",
    "\n",
    "If you are not familiar with Drake's hydroelastic contact, study [hydroelastic_contact_basics.ipynb](./hydroelastic_contact_basics.ipynb). You can also find more information in Hydroelastic Contact User Guide [here.](https://drake.mit.edu/doxygen_cxx/group__hydroelastic__user__guide.html)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Introduction\n",
    "\n",
    "This tutorial shows you how to set up simulations using compliant-hydroelastic nonconvex meshes. We'll use a simple example of a bell pepper dropped onto a bowl on a table top, with all three objects represented by compliant-hydroelastic meshes. Contact forces are calculated and visualized."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import os\n",
    "from pathlib import Path\n",
    "\n",
    "from IPython.display import Code\n",
    "\n",
    "from pydrake.geometry import StartMeshcat\n",
    "from pydrake.math import RigidTransform\n",
    "from pydrake.multibody.meshcat import ContactVisualizer, ContactVisualizerParams\n",
    "from pydrake.multibody.parsing import PackageMap, Parser\n",
    "from pydrake.multibody.plant import AddMultibodyPlant, MultibodyPlantConfig\n",
    "from pydrake.systems.analysis import Simulator\n",
    "from pydrake.systems.framework import DiagramBuilder\n",
    "from pydrake.visualization import ApplyVisualizationConfig, ModelVisualizer, VisualizationConfig"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Start MeshCat\n",
    "\n",
    "See the section [Viewing models](./authoring_multibody_simulation.ipynb#Viewing-models) in the tutorial [Authoring a Multibody Simulation](./authoring_multibody_simulation.ipynb) for an introduction to MeshCat."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Start the visualizer. The cell will output an HTTP link after the execution.\n",
    "# Click the link and a MeshCat tab should appear in your browser.\n",
    "meshcat = StartMeshcat()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Create compliant-hydroelastic bell pepper in SDFormat\n",
    "\n",
    "*Make sure you have the MeshCat tab opened in your browser; the link is shown immediately above.*\n",
    "\n",
    "We will load a compliant-hydroelastic bell pepper from an SDFormat file. We will show the file for you to read, and then use `ModelVisualizer` to display it.\n",
    "\n",
    "The file specifies inertia in the `<inertial>` block. See [mesh_to_model](https://drake.mit.edu/pydrake/pydrake.multibody.mesh_to_model.html) to compute the inertia matrix.\n",
    "\n",
    "The file specifies visual geometry using a triangle surface mesh and a collision geometry using a tetrahedral volume mesh from a VTK file.\n",
    "\n",
    "The file's `<drake:proximity_properties>` stanza will control hydroelastic contacts. Look in particular at the `<drake:hydroelastic_modulus>`.\n",
    "\n",
    "In the MeshCat tab, you should toggle the \"proximity\" checkbox to show the collision geometry, which is the tetrahedral mesh that fits the visual geometry's triangle mesh.  See the section *Viewing models* in [authoring_multibody_simulation.ipynb](./authoring_multibody_simulation.ipynb) for more details."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Show the contents of the SDFormat file.\n",
    "bell_pepper_url = \"package://drake_models/veggies/yellow_bell_pepper_no_stem_low.sdf\"\n",
    "bell_pepper_str = Path(PackageMap().ResolveUrl(bell_pepper_url)).read_text(encoding=\"utf-8\")\n",
    "Code(bell_pepper_str, language=\"xml\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Visualize the SDFormat file you just defined.\n",
    "visualizer = ModelVisualizer(meshcat=meshcat)\n",
    "visualizer.AddModels(url=bell_pepper_url)\n",
    "visualizer.Run(loop_once=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Create compliant-hydroelastic bowl in URDF\n",
    "\n",
    "We will load a compliant-hydroelastic bowl with URDF file. We will show the file for you to read, and then use `ModelVisualizer` to display it.\n",
    "\n",
    "The file specifies inertia in the `<inertial>` block. See [mesh_to_model](https://drake.mit.edu/pydrake/pydrake.multibody.mesh_to_model.html) to compute the inertia matrix.\n",
    "\n",
    "The file specifies visual geometry using a triangle surface mesh and a collision geometry using a tetrahedral volume mesh from a VTK file.\n",
    "\n",
    "In the `<drake:proximity_properties>` block, the `<drake:hydroelastic_modulus>` is set to 1e7 Pascals, so the bowl is stiffer than the bell pepper."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Show the contents of the URDF file.\n",
    "bowl_url = \"package://drake_models/dishes/evo_bowl_compliant.urdf\"\n",
    "bowl_str = Path(PackageMap().ResolveUrl(bowl_url)).read_text(encoding=\"utf-8\")\n",
    "Code(bowl_str, language=\"xml\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# (Drake users can ignore this cell. When Drake regression testing runs in CI,\n",
    "# we need to use a coarser tetrahedral mesh to improve debug performance.)\n",
    "test_mode = \"TEST_SRCDIR\" in os.environ\n",
    "if test_mode:\n",
    "    bowl_str = bowl_str.replace(\"evo_bowl_fine44k.vtk\", \n",
    "                                \"evo_bowl_coarse3k.vtk\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Visualize the URDF file.\n",
    "visualizer = ModelVisualizer(meshcat=meshcat)\n",
    "visualizer.parser().AddModels(file_contents=bowl_str, file_type=\"urdf\")\n",
    "visualizer.Run(loop_once=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Create compliant-hydroelastic table top in URDF\n",
    "\n",
    "The following URDF file specifies a compliant-hydroelastic box for a table top.  We demonstrate how to set relevant hydroelastic properties in URDF; however, Drake prefers SDFormat to URDF.\n",
    "\n",
    "Both the `<visual>` and `<collision>` geometries are boxes of the same size.\n",
    "\n",
    "In the `<drake:proximity_properties>` block, we will set `<drake:hydroelastic_modulus>` to 1e7 Pascals.\n",
    "\n",
    "We do not specify the inertia matrix of the table top because, in the next section when we set up `Diagram`, we will fix the table top to the world frame. It will not move."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Show the contents of the URDF file.\n",
    "table_top_url = \"package://drake_models/dishes/table_top.urdf\"\n",
    "table_top_str = Path(PackageMap().ResolveUrl(table_top_url)).read_text(encoding=\"utf-8\")\n",
    "Code(table_top_str, language=\"xml\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Visualize the URDF file.\n",
    "visualizer = ModelVisualizer(meshcat=meshcat)\n",
    "visualizer.AddModels(url=table_top_url)\n",
    "visualizer.Run(loop_once=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Create Diagram of the scene\n",
    "\n",
    "The function `add_scene()` below will create a scene using the assets that we created. It will use `Parser` to add the URDF and SDFormat strings into the scene. After this step, the next section will add visualization."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def add_scene(time_step):\n",
    "    builder = DiagramBuilder()\n",
    "    plant, scene_graph = AddMultibodyPlant(\n",
    "        MultibodyPlantConfig(\n",
    "            time_step=time_step,\n",
    "            discrete_contact_approximation=\"lagged\"),\n",
    "        builder)\n",
    "    parser = Parser(plant)\n",
    "\n",
    "    # Load the assets that we created.\n",
    "    parser.AddModels(url=bell_pepper_url)\n",
    "    parser.AddModels(file_contents=bowl_str, file_type=\"urdf\")\n",
    "    parser.AddModels(url=table_top_url)\n",
    "\n",
    "    # Weld the table top to the world so that it's fixed during simulation.\n",
    "    # The top surface passes the world's origin.\n",
    "    plant.WeldFrames(plant.world_frame(), \n",
    "                     plant.GetFrameByName(\"top_surface\"))\n",
    "\n",
    "    # Finalize the plant after loading the scene.\n",
    "    plant.Finalize()\n",
    "\n",
    "    # Place the bowl on top of the table.\n",
    "    X_WB = RigidTransform(p=[0, 0, 0.03])\n",
    "    plant.SetDefaultFreeBodyPose(plant.GetBodyByName(\"bowl\"), X_WB)\n",
    "    \n",
    "    # Drop the bell pepper from above the rim of the bowl. \n",
    "    X_WC = RigidTransform(p=[-0.06, 0, 0.30])\n",
    "    plant.SetDefaultFreeBodyPose(plant.GetBodyByName(\"yellow_bell_pepper_no_stem\"), X_WC)\n",
    "\n",
    "    return builder, plant"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Set up visualization\n",
    "\n",
    "The function `add_viz()` below will create visualization. First we will call `ApplyVisualizationConfig()` to visualize our assets. At this step we will set `publish_contacts=False`, so we can customize contact visualization afterwards. \n",
    "\n",
    "To visualize contact result, we will add `ContactVisualizer` with `newtons_per_meter= 20` and `newtons_meters_per_meter= 0.1`. It will draw a red arrow of length 1 meter for each force of 20 newtons and a blue arrow of length 1 meter for each torque of 0.1 newton\\*meters. The next section will run the simulation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def add_viz(builder, plant):\n",
    "    ApplyVisualizationConfig(\n",
    "        builder=builder, meshcat=meshcat,\n",
    "        config=VisualizationConfig(\n",
    "                 publish_contacts=False))    \n",
    "    ContactVisualizer.AddToBuilder(\n",
    "        builder=builder, plant=plant, meshcat=meshcat,\n",
    "        params=ContactVisualizerParams(\n",
    "                 newtons_per_meter=20,\n",
    "                 newton_meters_per_meter= 0.1))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Run simulation\n",
    "\n",
    "We will run the simulation. In MeshCat, the red arrow will represent the force `f`, and the blue arrow will represent the torque `tau`. You should see the contact patch moving around together with the force and torque vectors.\n",
    "\n",
    "After running the code below, playback with `timeScale` = 0.1 to appreciate the contact dynamics. You should see the force and torque vectors oscillate synchronously with the rocking bell pepper and bowl. See the section *Playback recording of the simulation* in [hydroelastic_contact_basics.ipynb](./hydroelastic_contact_basics.ipynb) for more details.\n",
    "\n",
    "Currently playing back the simulation will show contact force and torque correctly; however, it does not show contact patch appropriately, which could be confusing. Issue [19142](https://github.com/RobotLocomotion/drake/issues/19142) explains the problem in more details."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Clear MeshCat window from the previous blocks.\n",
    "meshcat.Delete()\n",
    "meshcat.DeleteAddedControls()\n",
    "\n",
    "time_step = 1e-2\n",
    "builder, plant = add_scene(time_step)\n",
    "add_viz(builder, plant)\n",
    "\n",
    "diagram = builder.Build()\n",
    "\n",
    "simulator = Simulator(diagram)\n",
    "\n",
    "# In interactive mode, simulate for longer time.\n",
    "# In test mode, simulate for shorter time.\n",
    "sim_time = 2 if not test_mode else 0.01\n",
    "\n",
    "meshcat.StartRecording()\n",
    "simulator.set_target_realtime_rate(1)\n",
    "simulator.AdvanceTo(sim_time)\n",
    "meshcat.StopRecording()\n",
    "meshcat.PublishRecording()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Download simulation result into a html file for sharing\n",
    "\n",
    "You can download the simulation result into a self-contained html file, allowing others to playback the simulated results without simulating. The following code prints the URL for downloading. Click on the printed URL to download."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(f\"{meshcat.web_url()}/download\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Further reading\n",
    "\n",
    "* [Hydroelastic Contact User Guide](https://drake.mit.edu/doxygen_cxx/group__hydroelastic__user__guide.html)\n",
    "\n",
    "* Elandt, R., Drumwright, E., Sherman, M., & Ruina, A. (2019, November). A pressure field model for fast, robust approximation of net contact force and moment between nominally rigid objects. In 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS) (pp. 8238-8245). IEEE. [link](https://arxiv.org/abs/1904.11433)\n",
    "\n",
    "* Masterjohn, J., Guoy, D., Shepherd, J., & Castro, A. (2022). Velocity Level Approximation of Pressure Field Contact Patches. IEEE Robotics and Automation Letters 7, no. 4 (2022): 11593-11600. [link](https://arxiv.org/abs/2110.04157v2)\n",
    "\n",
    "* Elandt, R. (2022, December). Pressure Field Contact. Dissertation. Cornell University. [link](https://ecommons.cornell.edu/handle/1813/112919)"
   ]
  }
 ],
 "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": 2
}