{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# Authoring a Multibody Simulation\n", "For instructions on how to run these tutorial notebooks, please see the [index](./index.ipynb).\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "This tutorial provides some tools to help you create a new scene description file that can be parsed into Drake's multibody physics engine (MultibodyPlant) and geometry engine (SceneGraph)." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Creating (or porting) a new robot/object in URDF or SDFormat\n", "\n", "The most important formats for creating multibody scenarios in Drake are the [Unified Robot Description Format (URDF)](http://wiki.ros.org/urdf) and the [Simulation Description Format (SDFormat)](http://sdformat.org/).\n", "\n", "They are both XML formats to describe robots or objects for robot simulators or visualization, and are fairly similar in syntax.\n", "\n", "In a high-level sense, you express different components of your robot using `` tags and connect them via `` tags. Each `` has three major subtags, ``, ``, and ``, for its visualization, planning/collision checking, and dynamics aspects. For `` and ``, you can either use primitives (box, sphere, cylinder, etc.) or meshes (.obj, .stl, and .dae) to represent the underlying geometry. \n", "\n", "Here are some useful resources specifically for [URDF](http://wiki.ros.org/urdf/Tutorials/Building%20a%20Visual%20Robot%20Model%20with%20URDF%20from%20Scratch) and [SDFormat](https://classic.gazebosim.org/tutorials?tut=build_model) creation.\n", "\n", "### URDF vs. SDFormat\n", "\n", "While URDF is the standardized format in ROS, it's lacking many features to describe a more complex scene. For example, URDF can only specify the kinematic and dynamic properties of a single robot in isolation. It can't specify joint loops and friction properties. Additionally, it can't specify things that are not robots, such as lights, heightmaps, etc.\n", "\n", "SDFormat was created to solve the shortcomings of URDF. SDFormat is a complete description for everything from the world level down to the robot level. The scalability makes it more suitable for sophisticated simulations.\n", "\n", "This tutorial will primarily focus on leveraging SDFormat, but the differences in using URDF should be minimal with some syntax changes.\n", "\n", "### Mesh file formats\n", "\n", "To use a mesh file for any of your robot `` entries, OBJ (`.obj`) is currently the best-supported format in Drake. If you have other file formats instead, [Meshlab](https://www.meshlab.net/), an open-source software, is a handy tool to convert common formats to a `.obj` for you." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Import some basic libraries and functions for this tutorial.\n", "import numpy as np\n", "import os\n", "\n", "from pydrake.common import FindResourceOrThrow, temp_directory\n", "from pydrake.geometry import (\n", " MeshcatVisualizer,\n", " MeshcatVisualizerParams,\n", " Role,\n", " StartMeshcat,\n", ")\n", "from pydrake.math import RigidTransform, RollPitchYaw\n", "from pydrake.multibody.meshcat import JointSliders\n", "from pydrake.multibody.parsing import Parser\n", "from pydrake.multibody.plant import AddMultibodyPlantSceneGraph\n", "from pydrake.systems.analysis import Simulator\n", "from pydrake.systems.framework import DiagramBuilder" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Create a Drake temporary directory to store files.\n", "# Note: this tutorial will create two temporary files (cylinder.sdf and\n", "# table_top.sdf) under `/tmp/robotlocomotion_drake_xxxxxx` directory.\n", "temp_dir = temp_directory()\n", "\n", "# 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": [ "## Inspecting an URDF/SDFormat model in Drake with joint sliders\n", "\n", "*Note: do make sure you have the MeshCat tab opened in your browser at this point since the following steps rely on that for visualization.*\n", "\n", "To visualize the models, we provide a `model_inspector()` function. This helper function will be very useful as we start to produce our own robot description files, or port description files over from another simulator.\n", "\n", "Simply supply the model file as an argument to the `model_inspector()` function. You should be able to visualize each model in MeshCat when you click through the following code blocks." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "def model_inspector(filename):\n", " meshcat.Delete()\n", " meshcat.DeleteAddedControls()\n", " builder = DiagramBuilder()\n", "\n", " # Note: the time_step here is chosen arbitrarily.\n", " plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)\n", "\n", " # Load the file into the plant/scene_graph.\n", " parser = Parser(plant)\n", " parser.AddModelFromFile(filename)\n", " plant.Finalize()\n", "\n", " # Add two visualizers, one to publish the \"visual\" geometry, and one to\n", " # publish the \"collision\" geometry.\n", " visual = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat,\n", " MeshcatVisualizerParams(role=Role.kPerception, prefix=\"visual\"))\n", " collision = MeshcatVisualizer.AddToBuilder(\n", " builder, scene_graph, meshcat,\n", " MeshcatVisualizerParams(role=Role.kProximity, prefix=\"collision\"))\n", " # Disable the collision geometry at the start; it can be enabled by the\n", " # checkbox in the meshcat controls.\n", " meshcat.SetProperty(\"collision\", \"visible\", False)\n", "\n", " # Set the timeout to a small number in test mode. Otherwise, JointSliders\n", " # will run until \"Stop JointSliders\" button is clicked.\n", " default_interactive_timeout = 1.0 if \"TEST_SRCDIR\" in os.environ else None\n", " sliders = builder.AddSystem(JointSliders(meshcat, plant))\n", " diagram = builder.Build()\n", " sliders.Run(diagram, default_interactive_timeout)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Loading an existing SDFormat in Drake\n", "\n", "Drake's `drake::multibody::Parser` can take SDFormat or URDF model files and instantiate the corresponding `drake::multibody::MultibodyPlant` and `drake::geometry::SceneGraph` (optionally) objects that a Drake simulation can interact with.\n", "\n", "To do that, we can use a Drake helper function, `FindResourceOrThrow()`, with an SDFormat file path to locate models inside Drake's repository, and use `model_inspector()` to visualize them.\n", "\n", "Now here are a few examples. Be sure to check out both the visual and collision geometries using the MeshCat control panel! The checkboxes are under the `Scene/drake` menu.\n", "\n", "### Interacting with JointSliders\n", "\n", "When running `model_inspector()` function, we can interactively control the joints through `JointSliders`. If any joint is specified in your robot/object, you should see various sliding bars in the control panel. Try adjusting the sliding bars and observing the motion of your robot/object.\n", "\n", "When you finish visualizing a model, you *NEED* to click the 'Stop JointSliders' button in the control panel to proceed to the next step. Once the button is clicked, it will disappear from the panel to indicate that the function execution ends. " ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Press the 'Stop JointSliders' button in MeshCat to continue.\n", "iiwa7_model_file = FindResourceOrThrow(\n", " \"drake/manipulation/models/\"\n", " \"iiwa_description/iiwa7/iiwa7_with_box_collision.sdf\")\n", "model_inspector(iiwa7_model_file)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Press the 'Stop JointSliders' button in MeshCat to continue.\n", "schunk_wsg50_model_file = FindResourceOrThrow(\n", " \"drake/manipulation/models/\"\n", " \"wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf\")\n", "model_inspector(schunk_wsg50_model_file)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Creating your own simple SDFormat file\n", "\n", "Besides loading the existing SDFormat files in Drake, you can also create your own SDFormat file and visualize it in this tutorial.\n", "\n", "We can create a very simple SDFormat that contains one model with a single link. Inside the link, we declare the mass and inertia properties, along with a primitive cylinder for the visual and collision geometries.\n", "\n", "You can modify the snippet below to change the size or material property of the cylinder." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Create a simple cylinder model.\n", "cylinder_sdf_file = os.path.join(temp_dir, \"cylinder.sdf\")\n", "cylinder_sdf = \"\"\"\n", "\n", " \n", " 0 0 0 0 0 0\n", " \n", " \n", " 1.0\n", " \n", " 0.005833\n", " 0.0\n", " 0.0\n", " 0.005833\n", " 0.0\n", " 0.005\n", " \n", " \n", " \n", " \n", " \n", " 0.1\n", " 0.2\n", " \n", " \n", " \n", " \n", " \n", " \n", " 0.1\n", " 0.2\n", " \n", " \n", " \n", " 1.0 1.0 1.0 1.0\n", " \n", " \n", " \n", " \n", "\n", "\n", "\"\"\"\n", "\n", "with open(cylinder_sdf_file, \"w\") as f:\n", " f.write(cylinder_sdf)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Press the 'Stop JointSliders' button in Meshcat to continue.\n", "# Visualize the cylinder from a SDFormat file you just created.\n", "model_inspector(cylinder_sdf_file)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Visual and collision geometry\n", "\n", "In the KUKA arm example, if you toggle the `drake/collision` checkbox in the MeshCat control panel a couple of times, you should see white boxes enveloping the KUKA arm appear and disappear. Those are the collision geometries defined in `iiwa7_with_box_collision.sdf` that are usually consumed by a motion planning or collision checking module when running the simulation.\n", "\n", "Even though we can use the same mesh to represent both the visual and collision geometry, approximating a complex mesh, like the KUKA arm, by primitive shapes can reduce the computation enormously. It's easier to check whether two cylinders collide than two irregular cylinder-like meshes. For that reason, we tend to load mesh files as the visual geometry but utilize various primitives as the collision geometry.\n", "\n", "### Define collision geometry for your model\n", "\n", "As collision geometry is merely an approximation for the actual shape of your model, we want the approximation to be reasonably close to reality. A rule of thumb is to completely envelop the actual shape but not inflate it too much. For example, rather than trying to cover an L-shape model with one giant box, using two boxes or cylinders can actually better represent the shape.\n", "\n", "It's a balancing act between the fidelity of the approximation and the computation cycles saved. When in doubt, start with a rough approximation around the actual shape, and see if any undesired behavior is introduced. E.g., the robot thinks it's in a collision when it's apparently not. Identify the questionable part of the collision geometry and replace it with a more accurate approximation, and then iterate.\n", "\n", "### Use a mesh as collision geometry\n", "\n", "In some cases you need to have a detailed collision geometry for your simulation, e.g., in the case of dexterous manipulation for objects with an irregular shape, it might be justifiable to use a mesh as the collision geometry directly.\n", "\n", "When an OBJ mesh is served as the collision geometry for a basic contact model, i.e., the point contact model, Drake will internally compute the convex hull of the mesh and use that instead. If you need a non-convex collision geometry, it's suggested to decompose your mesh to various convex shapes via a convex decomposition tool. There are many similar tools available that are mostly thin wrappers on [V-HACD](https://github.com/kmammou/v-hacd/). Among all, [convex_decomp_to_sdf](https://github.com/gizatt/convex_decomp_to_sdf) is a wrapper that we often use for Drake.\n", "\n", "However, for a more complex contact model that Drake provides, such as the hydroelastic contact model, Drake can directly utilize the actual mesh for its contact force computation. Refer to [Hydroelastic user guide](https://drake.mit.edu/doxygen_cxx/group__hydroelastic__user__guide.html) for more information." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Drake extensions to SDFormat/URDF\n", "\n", "Hopefully, you now have a clear picture of how to create, load, and visualize basic SDFormat and URDF models in Drake via MeshCat.\n", "\n", "In Drake, we extend URDF and SDFormat to allow access to Drake-specific features by adding Drake's custom tags. In the following example, `drake:compliant_hydroelastic` custom tag is added under the `collision` tag to declare a different contact model for a particular geometry. On the other hand, there are also features in both formats that Drake's parser doesn't support. The parser will either issue a warning, ignore it silently, or a combination of both.\n", "\n", "Considering this is a more advanced topic, check [Drake's documentation](https://drake.mit.edu/doxygen_cxx/group__multibody__parsing.html) for a full list of supported and unsupported tags in both formats.\n", "\n", "```\n", "\n", " \n", " ...\n", " \n", " \n", " ...\n", " \n", " \n", " 0 0 0 0 0 0\n", " \n", " ...\n", " \n", " \n", " ...\n", " \n", " \n", " \n", "\n", "```" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Creating (or porting) a \"scene\" with multiple robots/objects\n", "\n", "Finally, we are going to look at a more realistic simulation that contains multiple objects interacting with each other. In the simulation, we will load three objects, i.e., a cracker box from Drake, and a custom cylinder and table we created in this tutorial.\n", "\n", "At the beginning of the simulation, two objects are posed at certain heights, and then they free fall to the tabletop with gravity.\n", "\n", "### Create a simplified table\n", "\n", "Similar to the cylinder example above, we create and save the XML content to an SDFormat file to use in our simulation." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Create a table top SDFormat model.\n", "table_top_sdf_file = os.path.join(temp_dir, \"table_top.sdf\")\n", "table_top_sdf = \"\"\"\n", "\n", " \n", " \n", " \n", " 0 0 0.445 0 0 0\n", " \n", " \n", " 0.55 1.1 0.05\n", " \n", " \n", " \n", " 0.9 0.8 0.7 1.0\n", " \n", " \n", " \n", " 0 0 0.445 0 0 0\n", " \n", " \n", " 0.55 1.1 0.05\n", " \n", " \n", " \n", " \n", " \n", " 0 0 0.47 0 0 0\n", " \n", " \n", "\n", "\n", "\"\"\"\n", "\n", "with open(table_top_sdf_file, \"w\") as f:\n", " f.write(table_top_sdf)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Drake terminology\n", "\n", "In Drake, a [`System`](https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_system.html) is the building block that has input/output ports to connect with other Systems. For example, MultibodyPlant and SceneGraph are both Systems. A [`Diagram`](https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_diagram.html) is used to represent a meta-system that may have several interconnected Systems that function collectively.\n", "\n", "Each System and Diagram has its own [`Context`](https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_context.html) to represent its state and will be updated as the simulation progresses.\n", "\n", "The Context and the Diagram are the only two pieces of information a simulator needs to run. Given the same Context of a Diagram, the simulation should be completely deterministic and repeatable.\n", "\n", "Refer to [Modeling Dynamical Systems](https://github.com/RobotLocomotion/drake/blob/master/tutorials/dynamical_systems.ipynb), which covers more details on the relevant topics.\n", "\n", "*Note: Drake uses [Doxygen C++ Documentation](https://drake.mit.edu/doxygen_cxx/index.html) as the primary API documentation, but it also provides [Python API documentation](https://drake.mit.edu/pydrake/) for Python users.*\n", "\n", "### Load different objects into a \"scene\"\n", "\n", "In the `create_scene()` function, we first create a `drake::multibody::MultibodyPlant`, a `drake::multibody::SceneGraph`, and a parser.\n", "\n", "The parser is used to load the models into a MultibodyPlant. One thing to note in this example is we fix (or \"weld\") the table with respect to the world while treating the cracker box and the cylinder as free bodies. Once the MultibodyPlant is all set up properly, the function returns a diagram that a Drake Simulator consumes (a default context is used in this case)." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "def create_scene(sim_time_step=0.0001):\n", " # Clean up MeshCat.\n", " meshcat.Delete()\n", " meshcat.DeleteAddedControls()\n", "\n", " builder = DiagramBuilder()\n", " plant, scene_graph = AddMultibodyPlantSceneGraph(\n", " builder, time_step=sim_time_step)\n", " parser = Parser(plant)\n", "\n", " # Loading models.\n", " # Load a cracker box from Drake. \n", " cracker_box = FindResourceOrThrow(\n", " \"drake/manipulation/models/ycb/sdf/003_cracker_box.sdf\")\n", " parser.AddModelFromFile(cracker_box)\n", " # Load the table top and the cylinder we created.\n", " parser.AddModelFromFile(cylinder_sdf_file)\n", " parser.AddModelFromFile(table_top_sdf_file)\n", "\n", " # Weld the table to the world so that it's fixed during the simulation.\n", " table_frame = plant.GetFrameByName(\"table_top_center\")\n", " plant.WeldFrames(plant.world_frame(), table_frame)\n", " # Finalize the plant after loading the scene.\n", " plant.Finalize()\n", " # We use the default context to calculate the transformation of the table\n", " # in world frame but this is NOT the context the Diagram consumes.\n", " plant_context = plant.CreateDefaultContext()\n", "\n", " # Set the initial pose for the free bodies, i.e., the custom box and the\n", " # cracker box.\n", " cylinder = plant.GetBodyByName(\"cylinder_link\")\n", " X_WorldTable = table_frame.CalcPoseInWorld(plant_context)\n", " X_TableCylinder = RigidTransform(\n", " RollPitchYaw(np.asarray([90, 0, 0]) * np.pi / 180), p=[0,0,0.5])\n", " X_WorldCylinder = X_WorldTable.multiply(X_TableCylinder)\n", " plant.SetDefaultFreeBodyPose(cylinder, X_WorldCylinder)\n", "\n", " cracker_box = plant.GetBodyByName(\"base_link_cracker\")\n", " X_TableCracker = RigidTransform(\n", " RollPitchYaw(np.asarray([45, 30, 0]) * np.pi / 180), p=[0,0,0.8])\n", " X_WorldCracker = X_WorldTable.multiply(X_TableCracker)\n", " plant.SetDefaultFreeBodyPose(cracker_box, X_WorldCracker)\n", "\n", " # Add visualizer to visualize the geometries.\n", " visualizer = MeshcatVisualizer.AddToBuilder(\n", " builder, scene_graph, meshcat,\n", " MeshcatVisualizerParams(role=Role.kPerception, prefix=\"visual\"))\n", "\n", " diagram = builder.Build()\n", " return diagram, visualizer" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Running a simple simulation\n", "\n", "We have everything we need to launch the simulator! Run the following code block to start the simulation and visualize it in your MeshCat tab.\n", "\n", "This simple simulation represents a passive system in that the objects fall purely due to gravity without other power sources. Did they do what you expect? Try adjusting the `sim_time_step` and rerun the simulation. Start with a small value and increase it gradually to see if that changes the behavior." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "def initialize_simulation(diagram):\n", " simulator = Simulator(diagram)\n", " simulator.Initialize()\n", " simulator.set_target_realtime_rate(1.)\n", " return simulator\n", "\n", "def run_simulation(sim_time_step):\n", " diagram, visualizer = create_scene(sim_time_step)\n", " simulator = initialize_simulation(diagram)\n", " visualizer.StartRecording()\n", " simulator.AdvanceTo(5.0)\n", " visualizer.PublishRecording()\n", "\n", "# Run the simulation with a small time step. Try gradually increasing it!\n", "run_simulation(sim_time_step=0.0001)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Debugging your MultibodyPlant/SceneGraph\n", "\n", "Sometimes people get surprising results, e.g., unreasonable behaviors in simulation or program crash, due to the discrepancy between the simulation setup and the real-world physics properties.\n", "\n", "### Debugging the inertial property\n", "One common scenario for that is a lack of inertial properties for some of the simulated objects. The time step of the simulation may become extremely small (e.g., < 0.001s) due to the poorly specified system. Alternatively, you may receive an error message about `Delta > 0` or a warning that the inertial matrix is not physically valid.\n", "\n", "Double-check the inertial properties, especially if the dynamic behavior is the focus of the simulation.\n", "\n", "### Debugging the mass property\n", "You don't need to specify the mass of an object if it's welded to the world. However, an error will be triggered if you have a movable object with zero mass as the simulation is not yet fully specified.\n", "\n", "Hint: Does the mass/inertia of the movable objects seem reasonable? Try modifying them and rerun the simulation to observe changes." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Next steps\n", "\n", "This tutorial helps you set up the physics (MultibodyPlant) and geometry engines (SceneGraph) and visualize the simulation in MechCat. However, most robotics simulations require more. Next, you might need to model the sensors, the low-level control system, and eventually even the high-level perception, planning, and control systems for a real-world robot platform.\n", "\n", "Here are some other resources to help you explore further.\n", "\n", "- [Drake MultibodyPlant](https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html)\n", "- [Drake SceneGraph](https://drake.mit.edu/doxygen_cxx/classdrake_1_1geometry_1_1_scene_graph.html)\n", "- [Introduction to the basic robot pick-and-place using Drake](https://manipulation.csail.mit.edu/pick.html)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [] } ], "metadata": { "kernelspec": { "display_name": "Python 3", "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.8.10" } }, "nbformat": 4, "nbformat_minor": 2 }