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).
The most important formats for creating multibody scenarios in Drake are the Unified Robot Description Format (URDF) and the Simulation Description Format (SDFormat).
They are both XML formats to describe robots or objects for robot simulators or visualization, and are fairly similar in syntax.
In a high-level sense, you express different components of your robot using <link>
tags and connect them via <joint>
tags. Each <link>
has three major subtags, <visual>
, <collision>
, and <inertial>
, for its visualization, planning/collision checking, and dynamics aspects. For <visual>
and <collision>
, you can either use primitives (box, sphere, cylinder, etc.) or meshes (.obj, .stl, and .dae) to represent the underlying geometry.
Here are some useful resources specifically for URDF and SDFormat creation.
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.
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.
This tutorial will primarily focus on leveraging SDFormat, but the differences in using URDF should be minimal with some syntax changes.
To use a mesh file for any of your robot <link>
entries, OBJ (.obj
) is currently the best-supported format in Drake. If you have other file formats instead, Meshlab, an open-source software, is a handy tool to convert common formats to a .obj
for you.
# Import some basic libraries and functions for this tutorial.
import numpy as np
import os
from pydrake.common import FindResourceOrThrow, temp_directory
from pydrake.geometry import (
MeshcatVisualizer,
MeshcatVisualizerParams,
Role,
StartMeshcat,
)
from pydrake.math import RigidTransform, RollPitchYaw
from pydrake.multibody.meshcat import JointSliders
from pydrake.multibody.parsing import Parser
from pydrake.multibody.plant import AddMultibodyPlantSceneGraph
from pydrake.systems.analysis import Simulator
from pydrake.systems.framework import DiagramBuilder
# Create a Drake temporary directory to store files.
# Note: this tutorial will create two temporary files (cylinder.sdf and
# table_top.sdf) under `/tmp/robotlocomotion_drake_xxxxxx` directory.
temp_dir = temp_directory()
# Start the visualizer. The cell will output an HTTP link after the execution.
# Click the link and a MeshCat tab should appear in your browser.
meshcat = StartMeshcat()
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.
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.
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.
def model_inspector(filename):
meshcat.Delete()
meshcat.DeleteAddedControls()
builder = DiagramBuilder()
# Note: the time_step here is chosen arbitrarily.
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)
# Load the file into the plant/scene_graph.
parser = Parser(plant)
parser.AddModelFromFile(filename)
plant.Finalize()
# Add two visualizers, one to publish the "visual" geometry, and one to
# publish the "collision" geometry.
visual = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat,
MeshcatVisualizerParams(role=Role.kPerception, prefix="visual"))
collision = MeshcatVisualizer.AddToBuilder(
builder, scene_graph, meshcat,
MeshcatVisualizerParams(role=Role.kProximity, prefix="collision"))
# Disable the collision geometry at the start; it can be enabled by the
# checkbox in the meshcat controls.
meshcat.SetProperty("collision", "visible", False)
# Set the timeout to a small number in test mode. Otherwise, JointSliders
# will run until "Stop JointSliders" button is clicked.
default_interactive_timeout = 1.0 if "TEST_SRCDIR" in os.environ else None
sliders = builder.AddSystem(JointSliders(meshcat, plant))
diagram = builder.Build()
sliders.Run(diagram, default_interactive_timeout)
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.
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.
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.
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.
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.
# Press the 'Stop JointSliders' button in MeshCat to continue.
iiwa7_model_file = FindResourceOrThrow(
"drake/manipulation/models/"
"iiwa_description/iiwa7/iiwa7_with_box_collision.sdf")
model_inspector(iiwa7_model_file)
# Press the 'Stop JointSliders' button in MeshCat to continue.
schunk_wsg50_model_file = FindResourceOrThrow(
"drake/manipulation/models/"
"wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf")
model_inspector(schunk_wsg50_model_file)
Besides loading the existing SDFormat files in Drake, you can also create your own SDFormat file and visualize it in this tutorial.
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.
You can modify the snippet below to change the size or material property of the cylinder.
# Create a simple cylinder model.
cylinder_sdf_file = os.path.join(temp_dir, "cylinder.sdf")
cylinder_sdf = """<?xml version="1.0"?>
<sdf version="1.7">
<model name="cylinder">
<pose>0 0 0 0 0 0</pose>
<link name="cylinder_link">
<inertial>
<mass>1.0</mass>
<inertia>
<ixx>0.005833</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.005833</iyy>
<iyz>0.0</iyz>
<izz>0.005</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.2</length>
</cylinder>
</geometry>
</collision>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.2</length>
</cylinder>
</geometry>
<material>
<diffuse>1.0 1.0 1.0 1.0</diffuse>
</material>
</visual>
</link>
</model>
</sdf>
"""
with open(cylinder_sdf_file, "w") as f:
f.write(cylinder_sdf)
# Press the 'Stop JointSliders' button in Meshcat to continue.
# Visualize the cylinder from a SDFormat file you just created.
model_inspector(cylinder_sdf_file)
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.
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.
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.
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.
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.
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. Among all, convex_decomp_to_sdf is a wrapper that we often use for Drake.
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 for more information.
Hopefully, you now have a clear picture of how to create, load, and visualize basic SDFormat and URDF models in Drake via MeshCat.
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.
Considering this is a more advanced topic, check Drake's documentation for a full list of supported and unsupported tags in both formats.
<link name="example_link">
<inertial>
...
</inertial>
<visual name="example_visual">
...
</visual>
<collision name="example_collision">
<pose>0 0 0 0 0 0</pose>
<geometry>
...
</geometry>
<drake:proximity_properties>
...
<drake:compliant_hydroelastic/>
</drake:proximity_properties>
</collision>
</link>
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.
At the beginning of the simulation, two objects are posed at certain heights, and then they free fall to the tabletop with gravity.
Similar to the cylinder example above, we create and save the XML content to an SDFormat file to use in our simulation.
# Create a table top SDFormat model.
table_top_sdf_file = os.path.join(temp_dir, "table_top.sdf")
table_top_sdf = """<?xml version="1.0"?>
<sdf version="1.7">
<model name="table_top">
<link name="table_top_link">
<visual name="visual">
<pose>0 0 0.445 0 0 0</pose>
<geometry>
<box>
<size>0.55 1.1 0.05</size>
</box>
</geometry>
<material>
<diffuse>0.9 0.8 0.7 1.0</diffuse>
</material>
</visual>
<collision name="collision">
<pose>0 0 0.445 0 0 0</pose>
<geometry>
<box>
<size>0.55 1.1 0.05</size>
</box>
</geometry>
</collision>
</link>
<frame name="table_top_center">
<pose relative_to="table_top_link">0 0 0.47 0 0 0</pose>
</frame>
</model>
</sdf>
"""
with open(table_top_sdf_file, "w") as f:
f.write(table_top_sdf)
In Drake, a System
is the building block that has input/output ports to connect with other Systems. For example, MultibodyPlant and SceneGraph are both Systems. A Diagram
is used to represent a meta-system that may have several interconnected Systems that function collectively.
Each System and Diagram has its own Context
to represent its state and will be updated as the simulation progresses.
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.
Refer to Modeling Dynamical Systems, which covers more details on the relevant topics.
Note: Drake uses Doxygen C++ Documentation as the primary API documentation, but it also provides Python API documentation for Python users.
In the create_scene()
function, we first create a drake::multibody::MultibodyPlant
, a drake::multibody::SceneGraph
, and a parser.
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).
def create_scene(sim_time_step=0.0001):
# Clean up MeshCat.
meshcat.Delete()
meshcat.DeleteAddedControls()
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(
builder, time_step=sim_time_step)
parser = Parser(plant)
# Loading models.
# Load a cracker box from Drake.
cracker_box = FindResourceOrThrow(
"drake/manipulation/models/ycb/sdf/003_cracker_box.sdf")
parser.AddModelFromFile(cracker_box)
# Load the table top and the cylinder we created.
parser.AddModelFromFile(cylinder_sdf_file)
parser.AddModelFromFile(table_top_sdf_file)
# Weld the table to the world so that it's fixed during the simulation.
table_frame = plant.GetFrameByName("table_top_center")
plant.WeldFrames(plant.world_frame(), table_frame)
# Finalize the plant after loading the scene.
plant.Finalize()
# We use the default context to calculate the transformation of the table
# in world frame but this is NOT the context the Diagram consumes.
plant_context = plant.CreateDefaultContext()
# Set the initial pose for the free bodies, i.e., the custom box and the
# cracker box.
cylinder = plant.GetBodyByName("cylinder_link")
X_WorldTable = table_frame.CalcPoseInWorld(plant_context)
X_TableCylinder = RigidTransform(
RollPitchYaw(np.asarray([90, 0, 0]) * np.pi / 180), p=[0,0,0.5])
X_WorldCylinder = X_WorldTable.multiply(X_TableCylinder)
plant.SetDefaultFreeBodyPose(cylinder, X_WorldCylinder)
cracker_box = plant.GetBodyByName("base_link_cracker")
X_TableCracker = RigidTransform(
RollPitchYaw(np.asarray([45, 30, 0]) * np.pi / 180), p=[0,0,0.8])
X_WorldCracker = X_WorldTable.multiply(X_TableCracker)
plant.SetDefaultFreeBodyPose(cracker_box, X_WorldCracker)
# Add visualizer to visualize the geometries.
visualizer = MeshcatVisualizer.AddToBuilder(
builder, scene_graph, meshcat,
MeshcatVisualizerParams(role=Role.kPerception, prefix="visual"))
diagram = builder.Build()
return diagram, visualizer
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.
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.
def initialize_simulation(diagram):
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.)
return simulator
def run_simulation(sim_time_step):
diagram, visualizer = create_scene(sim_time_step)
simulator = initialize_simulation(diagram)
visualizer.StartRecording()
simulator.AdvanceTo(5.0)
visualizer.PublishRecording()
# Run the simulation with a small time step. Try gradually increasing it!
run_simulation(sim_time_step=0.0001)
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.
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.
Double-check the inertial properties, especially if the dynamic behavior is the focus of the simulation.
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.
Hint: Does the mass/inertia of the movable objects seem reasonable? Try modifying them and rerun the simulation to observe changes.
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.
Here are some other resources to help you explore further.