-
Notifications
You must be signed in to change notification settings - Fork 1.3k
/
simulate.cc
61 lines (49 loc) · 1.99 KB
/
simulate.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
#include <memory>
#include <gflags/gflags.h>
#include "drake/common/find_resource.h"
#include "drake/examples/compass_gait/compass_gait.h"
#include "drake/examples/compass_gait/compass_gait_geometry.h"
#include "drake/geometry/drake_visualizer.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram_builder.h"
namespace drake {
namespace examples {
namespace compass_gait {
namespace {
DEFINE_double(target_realtime_rate, 1.0,
"Playback speed. See documentation for "
"Simulator::set_target_realtime_rate() for details.");
/// Simulates the compass gait from various initial velocities (accepted as
/// command-line arguments. Run meldis to watch the results.
int DoMain() {
systems::DiagramBuilder<double> builder;
auto compass_gait = builder.AddSystem<CompassGait>();
compass_gait->set_name("compass_gait");
auto scene_graph = builder.AddSystem<geometry::SceneGraph>();
CompassGaitGeometry::AddToBuilder(&builder,
compass_gait->get_floating_base_state_output_port(), scene_graph);
geometry::DrakeVisualizerd::AddToBuilder(&builder, *scene_graph);
auto diagram = builder.Build();
systems::Simulator<double> simulator(*diagram);
systems::Context<double>& cg_context = diagram->GetMutableSubsystemContext(
*compass_gait, &simulator.get_mutable_context());
CompassGaitContinuousState<double>& state =
compass_gait->get_mutable_continuous_state(&cg_context);
state.set_stance(0.0);
state.set_swing(0.0);
state.set_stancedot(0.4);
state.set_swingdot(-2.0);
compass_gait->get_input_port(0).FixValue(&cg_context, Vector1d(0.0));
simulator.set_target_realtime_rate(FLAGS_target_realtime_rate);
simulator.get_mutable_context().SetAccuracy(1e-4);
simulator.AdvanceTo(10);
return 0;
}
} // namespace
} // namespace compass_gait
} // namespace examples
} // namespace drake
int main(int argc, char* argv[]) {
gflags::ParseCommandLineFlags(&argc, &argv, true);
return drake::examples::compass_gait::DoMain();
}