-
Notifications
You must be signed in to change notification settings - Fork 0
/
demo.cpp
95 lines (75 loc) · 3.06 KB
/
demo.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
#include <memory>
#include <gflags/gflags.h>
#include "drake/common/find_resource.h"
#include "pendulum_plant.h"
#include "drake/lcm/drake_lcm.h"
#include "drake/multibody/joints/floating_base_types.h"
#include "drake/multibody/parsers/urdf_parser.h"
#include "drake/multibody/rigid_body_plant/drake_visualizer.h"
#include "drake/multibody/rigid_body_tree.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/systems/primitives/constant_vector_source.h"
namespace drake {
namespace examples {
namespace pendulum {
namespace {
DEFINE_double(target_realtime_rate, 1.0,
"Playback speed. See documentation for "
"Simulator::set_target_realtime_rate() for details.");
/// Fixed path to particle URDF model (for visualization purposes only).
static const char* const kPendulumUrdfPath = "Pendulum.urdf";
/// Check if the specified file exists.
/// @param[in] name of the file
/// @return existence (true) or otherwise (false)
bool file_exists(const std::string& name) {
struct stat buffer;
return (stat(name.c_str(), &buffer) == 0);
}
int DoMain() {
lcm::DrakeLcm lcm;
auto tree = std::make_unique<RigidBodyTree<double>>();
if (!file_exists(kPendulumUrdfPath)) {
throw std::runtime_error(std::string("could not find '") +
kPendulumUrdfPath + std::string("'"));
}
parsers::urdf::AddModelInstanceFromUrdfFileToWorld(
kPendulumUrdfPath,
multibody::joints::kFixed, tree.get());
PendulumInput<double> input;
input.set_tau(0.0);
systems::DiagramBuilder<double> builder;
auto source = builder.AddSystem<systems::ConstantVectorSource>(input);
source->set_name("tau");
auto pendulum = builder.AddSystem<PendulumPlant>();
pendulum->set_name("pendulum");
auto publisher = builder.AddSystem<systems::DrakeVisualizer>(*tree, &lcm);
publisher->set_name("publisher");
builder.Connect(source->get_output_port(), pendulum->get_input_port());
builder.Connect(pendulum->get_output_port(), publisher->get_input_port(0));
auto diagram = builder.Build();
systems::Simulator<double> simulator(*diagram);
systems::Context<double>& pendulum_context =
diagram->GetMutableSubsystemContext(*pendulum,
&simulator.get_mutable_context());
PendulumState<double>& state = pendulum->get_mutable_state(&pendulum_context);
state.set_theta(1.);
state.set_thetadot(0.);
const double initial_energy = pendulum->CalcTotalEnergy(pendulum_context);
simulator.set_target_realtime_rate(FLAGS_target_realtime_rate);
simulator.Initialize();
simulator.StepTo(10);
const double final_energy = pendulum->CalcTotalEnergy(pendulum_context);
// Adds a numerical sanity test on total energy.
DRAKE_DEMAND(initial_energy > 2.0 * final_energy);
return 0;
}
} // namespace
} // namespace pendulum
} // namespace examples
} // namespace drake
int main(int argc, char* argv[]) {
gflags::ParseCommandLineFlags(&argc, &argv, true);
return drake::examples::pendulum::DoMain();
}