-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
rod2d_sim.cc
91 lines (78 loc) · 3.34 KB
/
rod2d_sim.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
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
#include <cmath>
#include <iomanip>
#include <iostream>
#include <limits>
#include <memory>
#include <gflags/gflags.h>
#include "drake/examples/rod2d/rod2d.h"
#include "drake/examples/rod2d/rod2d_geometry.h"
#include "drake/geometry/drake_visualizer.h"
#include "drake/systems/analysis/implicit_euler_integrator.h"
#include "drake/systems/analysis/runge_kutta3_integrator.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram.h"
#include "drake/systems/framework/diagram_builder.h"
using Rod2D = drake::examples::rod2d::Rod2D<double>;
using drake::examples::rod2d::Rod2dGeometry;
using drake::geometry::DrakeVisualizerd;
using drake::geometry::SceneGraph;
using drake::systems::Context;
using drake::systems::DiagramBuilder;
using drake::systems::ImplicitEulerIntegrator;
using drake::systems::RungeKutta3Integrator;
using drake::systems::Simulator;
// Simulation parameters.
DEFINE_string(system_type, "discretized",
"Type of rod system, valid values are "
"'discretized','continuous'");
DEFINE_double(dt, 1e-2, "Integration step size");
DEFINE_double(rod_radius, 5e-2, "Radius of the rod (for visualization only)");
DEFINE_double(sim_duration, 10, "Simulation duration in virtual seconds");
DEFINE_double(accuracy, 1e-5,
"Requested simulation accuracy (ignored for discretized system)");
int main(int argc, char* argv[]) {
// Parse any flags.
gflags::ParseCommandLineFlags(&argc, &argv, true);
DiagramBuilder<double> builder;
// Create the rod and add it to the diagram.
Rod2D* rod;
if (FLAGS_system_type == "discretized") {
rod = builder.template AddSystem<Rod2D>(
Rod2D::SystemType::kDiscretized, FLAGS_dt);
} else if (FLAGS_system_type == "continuous") {
rod = builder.template AddSystem<Rod2D>(Rod2D::SystemType::kContinuous,
0.0);
} else {
std::cerr << "Invalid simulation type '" << FLAGS_system_type
<< "'; note that types are case sensitive." << std::endl;
return -1;
}
auto& scene_graph = *builder.AddSystem<SceneGraph<double>>();
Rod2dGeometry::AddToBuilder(FLAGS_rod_radius, rod->get_rod_half_length() * 2,
&builder, rod->state_output(), &scene_graph);
DrakeVisualizerd::AddToBuilder(&builder, scene_graph);
// Set the names of the systems.
rod->set_name("rod");
auto diagram = builder.Build();
// Make no external forces act on the rod.
auto context = diagram->CreateDefaultContext();
Context<double>& rod_context =
diagram->GetMutableSubsystemContext(*rod, context.get());
const Eigen::Vector3d ext_input(0, 0, 0);
rod->get_input_port(0).FixValue(&rod_context, ext_input);
// Set up the integrator.
Simulator<double> simulator(*diagram, std::move(context));
if (FLAGS_system_type == "continuous") {
simulator.reset_integrator<ImplicitEulerIntegrator<double>>();
} else {
simulator.reset_integrator<RungeKutta3Integrator<double>>();
}
simulator.get_mutable_integrator().set_target_accuracy(FLAGS_accuracy);
simulator.get_mutable_integrator().set_maximum_step_size(FLAGS_dt);
// Start simulating.
simulator.set_target_realtime_rate(1.0);
while (simulator.get_context().get_time() < FLAGS_sim_duration) {
const double t = simulator.get_context().get_time();
simulator.AdvanceTo(std::min(t + 1, FLAGS_sim_duration));
}
}