{
"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": [
"## Scene file formats: URDF and 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. This 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 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.parsing import Parser\n",
"from pydrake.multibody.plant import AddMultibodyPlantSceneGraph\n",
"from pydrake.systems.analysis import Simulator\n",
"from pydrake.systems.framework import DiagramBuilder\n",
"from pydrake.visualization import ModelVisualizer"
]
},
{
"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": [
"## Viewing models\n",
"\n",
"*Make sure you have the MeshCat tab opened in your browser; the link is shown immediately above.*\n",
"\n",
"Drake provides a `ModelVisualizer` class to visualize models interactively. This class will help as we start to produce our own robot description files, or port description files over from another simulator. We'll show examples in the cells below, using a couple of pre-existing models provided by Drake.\n",
"\n",
"After running each of the two example cells, switch to the MeshCat tab to see the robot. Be sure to check out both the \"illustration\" and \"proximity\" geometries (the Drake names for visual and collision) using the MeshCat control panel. Click **Open Controls** to unfold the control panel. The geometry checkboxes are under the **Scene / drake** menu. Try adjusting the sliding bars to observe the kinematics of the robot."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# First we'll choose one of Drake's example model files, a KUKA iiwa arm.\n",
"iiwa7_model_url = (\n",
" \"package://drake/manipulation/models/\"\n",
" \"iiwa_description/iiwa7/iiwa7_with_box_collision.sdf\")\n",
"\n",
"# Create a model visualizer and add the robot arm.\n",
"visualizer = ModelVisualizer(meshcat=meshcat)\n",
"visualizer.parser().AddModels(url=iiwa7_model_url)\n",
"\n",
"# When this notebook is run in test mode it needs to stop execution without\n",
"# user interaction. For interactive model visualization you won't normally\n",
"# need the 'loop_once' flag.\n",
"test_mode = True if \"TEST_SRCDIR\" in os.environ else False\n",
"\n",
"# Start the interactive visualizer.\n",
"# Click the \"Stop Running\" button in MeshCat when you're finished.\n",
"visualizer.Run(loop_once=test_mode)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Choose another one of Drake's example model files, a Schunk WSG gripper.\n",
"schunk_wsg50_model_url = (\n",
" \"package://drake/manipulation/models/\"\n",
" \"wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf\")\n",
"\n",
"# Create a NEW model visualizer and add the robot gripper.\n",
"visualizer = ModelVisualizer(meshcat=meshcat)\n",
"visualizer.parser().AddModels(url=schunk_wsg50_model_url)\n",
"\n",
"# Start the interactive visualizer.\n",
"# Click the \"Stop Running\" button in MeshCat when you're finished.\n",
"visualizer.Run(loop_once=test_mode)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Creating custom models\n",
"Besides loading the existing SDFormat files in Drake, you can also create your own SDFormat model and visualize it in this tutorial. The data can be in a file or in a string.\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": [
"# Define a simple cylinder model.\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",
"\"\"\""
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"In addition to the `AddModels` method, the `ModelVisualizer` class provides access to its `Parser` object; you can access the full parser API to add models, e.g., from the string buffer we just created."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Visualize the cylinder from the SDFormat string you just defined.\n",
"visualizer = ModelVisualizer(meshcat=meshcat)\n",
"visualizer.parser().AddModelsFromString(cylinder_sdf, \"sdf\")\n",
"\n",
"# Click the \"Stop Running\" button in MeshCat when you're finished.\n",
"visualizer.Run(loop_once=test_mode)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Visual and collision geometry\n",
"\n",
"In the KUKA arm example, if you toggle the `drake/proximity` 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",
"### Creating an SDFormat wrapper around a mesh file\n",
"\n",
"You might have a mesh file of an object to manipulate and want to add it to a simulation. The easiest option is to pass the OBJ file to `Parser.AddModels` directly. Parsing it directly will use default assumptions for scale, mass, etc.\n",
"\n",
"In case those defaults are insufficient, you should create an SDFormat wrapper file to specify the additional properties (mass, inertia, scale, compliance, etc.) and load that file instead. You can use the [pydrake.multibody.mesh_to_model](https://drake.mit.edu/pydrake/pydrake.multibody.mesh_to_model.html) command-line tool to generate a baseline SDFormat file that you can then further customize.\n",
"\n",
"Another interesting up-and-coming option is [obj2mjcf](https://github.com/kevinzakka/obj2mjcf/), which also does mesh reprocessing. Drake can load MuJoCo XML files, but does not yet quite support enough of the MuJoCo file format to inter-operate well with obj2mjcf. You might be able to get it working with some fiddling.\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",
"This is similar to the cylinder example above but here 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 Drake temporary directory to store files.\n",
"# Note: this tutorial will create a temporary file (table_top.sdf)\n",
"# in the `/tmp/robotlocomotion_drake_xxxxxx` directory.\n",
"temp_dir = temp_directory()\n",
"\n",
"# 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 `pydrake.multibody.MultibodyPlant`, a `pydrake.multibody.SceneGraph`, and a `pydrake.multibody.parsing.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):\n",
" # Clean up the Meshcat instance.\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 the table top and the cylinder we created.\n",
" parser.AddModelsFromString(cylinder_sdf, \"sdf\")\n",
" parser.AddModels(table_top_sdf_file)\n",
" # Load a cracker box from Drake. \n",
" parser.AddModels(\n",
" url=\"package://drake/manipulation/models/ycb/sdf/003_cracker_box.sdf\")\n",
" # Load an OBJ file from Drake, with no SDFormat wrapper file. In this case,\n",
" # the mass and inertia are inferred based on the volume of the mesh as if\n",
" # it were filled with water, and the mesh is used for both collision and\n",
" # visual geometry.\n",
" parser.AddModels(\n",
" url=\"package://drake_models/ycb/meshes/004_sugar_box_textured.obj\")\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 cylinder,\n",
" # the cracker box, and the sugar 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",
" sugar_box = plant.GetBodyByName(\"004_sugar_box_textured\")\n",
" X_TableSugar = RigidTransform(p=[0,-0.25,0.8])\n",
" X_WorldSugar = X_WorldTable.multiply(X_TableSugar)\n",
" plant.SetDefaultFreeBodyPose(sugar_box, X_WorldSugar)\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? You can also use the **reset** and **play** buttons in the MeshCat tab to re-run the simulation.\n",
"\n",
"Try adjusting the `sim_time_step` and re-run 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",
" finish_time = 0.1 if test_mode else 2.0\n",
" simulator.AdvanceTo(finish_time)\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
}