-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
run_passive.cc
69 lines (57 loc) · 2.21 KB
/
run_passive.cc
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
#include <memory>
#include <gflags/gflags.h>
#include "drake/examples/acrobot/acrobot_geometry.h"
#include "drake/examples/acrobot/acrobot_plant.h"
#include "drake/examples/acrobot/gen/acrobot_state.h"
#include "drake/geometry/drake_visualizer.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram.h"
#include "drake/systems/framework/diagram_builder.h"
namespace drake {
namespace examples {
namespace acrobot {
namespace {
// Simple example which simulates the (passive) Acrobot. Run meldis to see
// the animated result.
DEFINE_double(simulation_sec, 10.0,
"Number of seconds to simulate.");
DEFINE_double(realtime_factor, 1.0,
"Playback speed. See documentation for "
"Simulator::set_target_realtime_rate() for details.");
int do_main() {
systems::DiagramBuilder<double> builder;
auto acrobot = builder.AddSystem<AcrobotPlant>();
acrobot->set_name("acrobot");
auto scene_graph = builder.AddSystem<geometry::SceneGraph>();
AcrobotGeometry::AddToBuilder(
&builder, acrobot->get_output_port(0), scene_graph);
geometry::DrakeVisualizerd::AddToBuilder(&builder, *scene_graph);
auto diagram = builder.Build();
systems::Simulator<double> simulator(*diagram);
systems::Context<double>& acrobot_context =
diagram->GetMutableSubsystemContext(*acrobot,
&simulator.get_mutable_context());
const double tau = 0;
acrobot->GetInputPort("elbow_torque").FixValue(&acrobot_context, tau);
// Set an initial condition that is sufficiently far from the downright fixed
// point.
AcrobotState<double>* x0 = dynamic_cast<AcrobotState<double>*>(
&acrobot_context.get_mutable_continuous_state_vector());
DRAKE_DEMAND(x0 != nullptr);
x0->set_theta1(1.0);
x0->set_theta2(1.0);
x0->set_theta1dot(0.0);
x0->set_theta2dot(0.0);
simulator.set_target_realtime_rate(FLAGS_realtime_factor);
simulator.Initialize();
simulator.AdvanceTo(FLAGS_simulation_sec);
return 0;
}
} // namespace
} // namespace acrobot
} // namespace examples
} // namespace drake
int main(int argc, char* argv[]) {
gflags::ParseCommandLineFlags(&argc, &argv, true);
return drake::examples::acrobot::do_main();
}