/// @brief An SDF based double pendulum example.
#include <gflags/gflags.h>
#include "drake/common/drake_assert.h"
#include "drake/common/find_resource.h"
#include "drake/common/text_logging_gflags.h"
#include "drake/geometry/geometry_visualization.h"
#include "drake/geometry/scene_graph.h"
#include "drake/lcm/drake_lcm.h"
#include "drake/multibody/parsing/parser.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram.h"
#include "drake/systems/framework/diagram_builder.h"
DEFINE_double(target_realtime_rate, 1.0,
"Rate at which to run the simulation, relative to realtime");
DEFINE_double(simulation_time, 10, "How long to simulate the pendulum");
DEFINE_double(max_time_step, 1.0e-3,
"Simulation time step used for integrator.");
namespace double_pendulum {
// Fixed path to double pendulum SDF model.
static const char* const kDoublePendulumSdfPath =
"drake/examples/double_pendulum_pid/models/double_pendulum.sdf";
// Main function for demo.
DRAKE_DEMAND(FLAGS_simulation_time > 0);
systems::DiagramBuilder<double> builder;
geometry::SceneGraph<double>& scene_graph =
*builder.AddSystem<geometry::SceneGraph>();
scene_graph.set_name("scene_graph");
// Load and parse double pendulum SDF from file into a tree.
multibody::MultibodyPlant<double>* dp =
builder.AddSystem<multibody::MultibodyPlant<double>>(FLAGS_max_time_step);
dp->RegisterAsSourceForSceneGraph(&scene_graph);
multibody::Parser parser(dp);
const std::string sdf_path = FindResourceOrThrow(kDoublePendulumSdfPath);
multibody::ModelInstanceIndex plant_model_instance_index =
parser.AddModelFromFile(sdf_path);
(void)plant_model_instance_index;
// Weld the base link to world frame with no rotation.
const auto& root_link = dp->GetBodyByName("base");
dp->AddJoint<multibody::WeldJoint>("weld_base", dp->world_body(), nullopt,
Isometry3<double>::Identity());
// Now the plant is complete.
// Connect plant with scene_graph to get collision information.
DRAKE_DEMAND(!!dp->get_source_id());
dp->get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(dp->get_source_id().value()));
builder.Connect(scene_graph.get_query_output_port(),
dp->get_geometry_query_input_port());
geometry::ConnectDrakeVisualizer(&builder, scene_graph);
auto diagram = builder.Build();
std::unique_ptr<systems::Context<double>> diagram_context =
diagram->CreateDefaultContext();
// Create plant_context to set velocity.
systems::Context<double>& plant_context =
diagram->GetMutableSubsystemContext(*dp, diagram_context.get());
Eigen::VectorXd positions = Eigen::VectorXd::Zero(2);
dp->SetPositions(&plant_context, positions);
systems::Simulator<double> simulator(*diagram, std::move(diagram_context));
simulator.set_publish_every_time_step(true);
simulator.set_target_realtime_rate(FLAGS_target_realtime_rate);
simulator.AdvanceTo(FLAGS_simulation_time);
} // namespace double_pendulum
int main(int argc, char** argv) {
"//examples/double_pendulum_pid:run_double_pendulum_passive_exe");
gflags::ParseCommandLineFlags(&argc, &argv, true);
drake::examples::double_pendulum::DoMain();