Create a URDF/SDF robot

Search…
To create a Multibody robot, read from a robot description file is a convenient way. Drake support parsing URDF and SDF files.
Run the demo
The code is also available on my github, you could also skip to the result.
Create workspace & get model
1
cd drake
2
mkdir -p examples/double_pendulum_pid/models
3
curl -o examples/double_pendulum_pid/models/double_pendulum.sdf https://raw.githubusercontent.com/guzhaoyuan/drake/tutorial/examples/double_pendulum_pid/models/double_pendulum.sdf
Copied!
Create the source file
BUILD.bazel
1
load(
2
    "@drake//tools/skylark:drake_cc.bzl",
3
    "drake_cc_binary",
4
)
5
load("//tools/install:install_data.bzl", "install_data")
6
​
7
drake_cc_binary(
8
    name = "run_double_pendulum_passive_exe",
9
    srcs = [
10
        "run_double_pendulum_passive.cc",
11
    ],
12
    data = [
13
        ":models",
14
        "//tools:drake_visualizer",
15
    ],
16
    deps = [
17
        "//common:find_resource",
18
        "//common:text_logging_gflags",
19
        "//geometry:geometry_visualization",
20
        "//lcm",
21
        "//multibody/parsing",
22
        "//multibody/plant",
23
        "//systems/analysis",
24
        "//systems/framework",
25
        "@sdformat",
26
    ],
27
)
28
​
29
install_data()
Copied!
run_double_pendulum_passive.cc
1
///
2
/// @brief  An SDF based double pendulum example.
3
///
4
​
5
#include <gflags/gflags.h>
6
​
7
#include "drake/common/drake_assert.h"
8
#include "drake/common/find_resource.h"
9
#include "drake/common/text_logging_gflags.h"
10
#include "drake/geometry/geometry_visualization.h"
11
#include "drake/geometry/scene_graph.h"
12
#include "drake/lcm/drake_lcm.h"
13
#include "drake/multibody/parsing/parser.h"
14
#include "drake/multibody/plant/multibody_plant.h"
15
#include "drake/systems/analysis/simulator.h"
16
#include "drake/systems/framework/diagram.h"
17
#include "drake/systems/framework/diagram_builder.h"
18
​
19
DEFINE_double(target_realtime_rate, 1.0,
20
              "Rate at which to run the simulation, relative to realtime");
21
DEFINE_double(simulation_time, 10, "How long to simulate the pendulum");
22
DEFINE_double(max_time_step, 1.0e-3,
23
              "Simulation time step used for integrator.");
24
​
25
namespace drake {
26
namespace examples {
27
namespace double_pendulum {
28
namespace {
29
​
30
// Fixed path to double pendulum SDF model.
31
static const char* const kDoublePendulumSdfPath =
32
    "drake/examples/double_pendulum_pid/models/double_pendulum.sdf";
33
​
34
//
35
// Main function for demo.
36
//
37
void DoMain() {
38
  DRAKE_DEMAND(FLAGS_simulation_time > 0);
39
​
40
  systems::DiagramBuilder<double> builder;
41
​
42
  geometry::SceneGraph<double>& scene_graph =
43
      *builder.AddSystem<geometry::SceneGraph>();
44
  scene_graph.set_name("scene_graph");
45
​
46
  // Load and parse double pendulum SDF from file into a tree.
47
  multibody::MultibodyPlant<double>* dp =
48
      builder.AddSystem<multibody::MultibodyPlant<double>>(FLAGS_max_time_step);
49
  dp->set_name("plant");
50
  dp->RegisterAsSourceForSceneGraph(&scene_graph);
51
​
52
  multibody::Parser parser(dp);
53
  const std::string sdf_path = FindResourceOrThrow(kDoublePendulumSdfPath);
54
  multibody::ModelInstanceIndex plant_model_instance_index =
55
      parser.AddModelFromFile(sdf_path);
56
  (void)plant_model_instance_index;
57
​
58
  // Weld the base link to world frame with no rotation.
59
  const auto& root_link = dp->GetBodyByName("base");
60
  dp->AddJoint<multibody::WeldJoint>("weld_base", dp->world_body(), nullopt,
61
                                     root_link, nullopt,
62
                                     Isometry3<double>::Identity());
63
  // Now the plant is complete.
64
  dp->Finalize();
65
​
66
  // Connect plant with scene_graph to get collision information.
67
  DRAKE_DEMAND(!!dp->get_source_id());
68
  builder.Connect(
69
      dp->get_geometry_poses_output_port(),
70
      scene_graph.get_source_pose_port(dp->get_source_id().value()));
71
  builder.Connect(scene_graph.get_query_output_port(),
72
                  dp->get_geometry_query_input_port());
73
​
74
  geometry::ConnectDrakeVisualizer(&builder, scene_graph);
75
​
76
  auto diagram = builder.Build();
77
  std::unique_ptr<systems::Context<double>> diagram_context =
78
      diagram->CreateDefaultContext();
79
​
80
  // Create plant_context to set velocity.
81
  systems::Context<double>& plant_context =
82
      diagram->GetMutableSubsystemContext(*dp, diagram_context.get());
83
​
84
  // Set init position.
85
  Eigen::VectorXd positions = Eigen::VectorXd::Zero(2);
86
  positions[0] = 0.1;
87
  positions[1] = 0.4;
88
  dp->SetPositions(&plant_context, positions);
89
​
90
  systems::Simulator<double> simulator(*diagram, std::move(diagram_context));
91
  simulator.set_publish_every_time_step(true);
92
  simulator.set_target_realtime_rate(FLAGS_target_realtime_rate);
93
  simulator.Initialize();
94
  simulator.AdvanceTo(FLAGS_simulation_time);
95
}
96
​
97
}  // namespace
98
}  // namespace double_pendulum
99
}  // namespace examples
100
}  // namespace drake
101
​
102
int main(int argc, char** argv) {
103
  gflags::SetUsageMessage(
104
      "bazel run "
105
      "//examples/double_pendulum_pid:run_double_pendulum_passive_exe");
106
  gflags::ParseCommandLineFlags(&argc, &argv, true);
107
  drake::examples::double_pendulum::DoMain();
108
  return 0;
109
}
Copied!
Run the code
1
bazel-bin/tools/drake_visualizer &
2
bazel run //examples/double_pendulum_pid:run_double_pendulum_passive_exe
Copied!
The Code Explained
BUILD.bazel
1
install_data()
Copied!
The install_data() copy the items under the data tag to the build folder so when the program actually runs, it will be able to access the data files.
run_double_pendulum_passive.cc
1
DRAKE_DEMAND(FLAGS_simulation_time > 0);
Copied!
DRAKE_DEMAND is an assertion that ensures that the FLAGS_simulation_time is larger than zero. If not fulfilled, the program will stop throwing an exception. We could always use DRAKE_DEMAND to verify data correctness. 
run_double_pendulum_passive.cc
1
systems::DiagramBuilder<double> builder;
Copied!
Create a DiagramBuilder that helps to add system blocks and connect different systems.
run_double_pendulum_passive.cc
1
geometry::SceneGraph<double>& scene_graph =
2
    *builder.AddSystem<geometry::SceneGraph>();
3
scene_graph.set_name("scene_graph");
Copied!
Add a SceneGraph<double> to the diagram using builder. Note that system have we added was recorded and stored. The diagram and all the system are created when we call:
run_double_pendulum_passive.cc
1
auto diagram = builder.Build();
Copied!
run_double_pendulum_passive.cc
1
// Load and parse double pendulum SDF from file into a tree.
2
multibody::MultibodyPlant<double>* dp =
3
  builder.AddSystem<multibody::MultibodyPlant<double>>(FLAGS_max_time_step);
4
dp->set_name("plant");
5
dp->RegisterAsSourceForSceneGraph(&scene_graph);
Copied!
Create a MultibodyPlant<double> that represents the double pendulum. Then we set its name and connect it to SceneGraph so we could see the model later in simulation.
run_double_pendulum_passive.cc
1
multibody::Parser parser(dp);
2
const std::string sdf_path = FindResourceOrThrow(kDoublePendulumSdfPath);
3
multibody::ModelInstanceIndex plant_model_instance_index =
4
  parser.AddModelFromFile(sdf_path);
5
(void)plant_model_instance_index;
Copied!
Here we create a parser that actually create the model. It will parse the provided URDF or SDF file line by line, and call Drake functions internally to create a multibody in tree structure.
run_double_pendulum_passive.cc
1
// Weld the base link to world frame with no rotation.
2
const auto& root_link = dp->GetBodyByName("base");
3
dp->AddJoint<multibody::WeldJoint>("weld_base", dp->world_body(), nullopt,
4
  root_link, nullopt,
5
  Isometry3<double>::Identity());
6
// Now the plant is complete.
7
dp->Finalize();  
8
​
9
DRAKE_DEMAND(!!dp->get_source_id());
Copied!
We then create a fixed joint WeldJoint between the robot base and the world frame. So the robot base always stay fixed on the ground. Once the MultibodyPlant is finished, we call dp->Finalize() so we seal the plant, making sure the robot model is not mutable during simulation. The DRAKE_DEMAND makes sure that the MultibodyPlant is created successfully.
run_double_pendulum_passive.cc
1
builder.Connect(
2
  dp->get_geometry_poses_output_port(),
3
  scene_graph.get_source_pose_port(dp->get_source_id().value()));
4
builder.Connect(scene_graph.get_query_output_port(),
5
                dp->get_geometry_query_input_port());
6
​
7
geometry::ConnectDrakeVisualizer(&builder, scene_graph);
Copied!
After the plant is created. We use builder again to connect the MultibodyPlant system with other blocks in the diagram. scene_graph receive the MultibodyPlant current state and compute the collision in the system. It then reports all the collision infomation to the MultibodyPlant so MultibodyPlant will decide the contact force according to the contact body material.
run_double_pendulum_passive.cc
1
auto diagram = builder.Build();
2
std::unique_ptr<systems::Context<double>> diagram_context =
3
  diagram->CreateDefaultContext();
4
​
5
// Create plant_context to set velocity.
6
systems::Context<double>& plant_context =
7
  diagram->GetMutableSubsystemContext(*dp, diagram_context.get());
Copied!
Create the diagram  and diagram_context. Get the plant_context by getting subsystem context from the diagram_context.
run_double_pendulum_passive.cc
1
// Set init position.
2
Eigen::VectorXd positions = Eigen::VectorXd::Zero(2);
3
positions[0] = 0.1;
4
positions[1] = 0.4;
5
dp->SetPositions(&plant_context, positions);
Copied!
Set the initial position of the robot away from the home position.
run_double_pendulum_passive.cc
1
systems::Simulator<double> simulator(*diagram, std::move(diagram_context));
2
simulator.set_publish_every_time_step(true);
3
simulator.set_target_realtime_rate(FLAGS_target_realtime_rate);
4
simulator.Initialize();
5
simulator.AdvanceTo(FLAGS_simulation_time);
Copied!
Create a Simulator , set the simulation parameter. Then actually do the simulation by advance to the desired time.
Quick access to the result
Add code to you repository.
1
cd drake
2
git remote add gzy https://github.com/guzhaoyuan/drake.git
3
git pull gzy tutorial
4
git checkout tutorial
Copied!
Run.
1
bazel-bin/tools/drake_visualizer &
2
bazel run //examples/double_pendulum_pid:run_double_pendulum_passive_exe
Copied!
Extensions
Similar example of parsing an Allegro hand.
1
bazel-bin/tools/drake_visualizer &
2
bazel run //examples/allegro_hand:run_allegro_constant_load_demo
Copied!
Things to do in drake - Previous
Hello, Drake!
Next - Things to do in drake
Visualize data in LCM
Last modified 2yr ago
Copy link
Contents
Run the demo
The Code Explained
Quick access to the result
Extensions