-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
run_quadrotor_dynamics.cc
86 lines (73 loc) · 2.74 KB
/
run_quadrotor_dynamics.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
/// @file
///
/// This demo sets up a passive Quadrotor plant in a world described by the
/// warehouse model. The robot simply passively falls to the floor within the
/// walls of the warehouse, falling from the initial_height command line
/// argument.
#include <gflags/gflags.h>
#include "drake/common/text_logging.h"
#include "drake/multibody/parsing/parser.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/systems/analysis/runge_kutta2_integrator.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/analysis/simulator_gflags.h"
#include "drake/systems/framework/diagram.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/systems/primitives/constant_vector_source.h"
#include "drake/visualization/visualization_config_functions.h"
namespace drake {
namespace examples {
namespace quadrotor {
namespace {
DEFINE_double(duration, 0.5, "Total duration of simulation.");
DEFINE_double(initial_height, 0.051, "Initial height of the Quadrotor.");
template <typename T>
class Quadrotor : public systems::Diagram<T> {
public:
Quadrotor() {
this->set_name("Quadrotor");
systems::DiagramBuilder<T> builder;
auto [plant, scene_graph] =
multibody::AddMultibodyPlantSceneGraph(&builder, 0.0);
multibody::Parser parser(&plant);
parser.AddModelsFromUrl(
"package://drake/examples/quadrotor/quadrotor.urdf");
parser.AddModelsFromUrl(
"package://drake/examples/quadrotor/warehouse.sdf");
plant.Finalize();
DRAKE_DEMAND(plant.num_actuators() == 0);
DRAKE_DEMAND(plant.num_positions() == 7);
visualization::AddDefaultVisualization(&builder);
builder.BuildInto(this);
plant_ = &plant;
}
void SetDefaultState(const systems::Context<T>& context,
systems::State<T>* state) const override {
DRAKE_DEMAND(state != nullptr);
systems::Diagram<T>::SetDefaultState(context, state);
const systems::Context<T>& plant_context =
this->GetSubsystemContext(*plant_, context);
systems::State<T>& plant_state =
this->GetMutableSubsystemState(*plant_, state);
const math::RigidTransform<T> X_WB(
Vector3<T>{0.0, 0.0, FLAGS_initial_height});
plant_->SetFreeBodyPose(
plant_context, &plant_state, plant_->GetBodyByName("base_link"), X_WB);
}
private:
multibody::MultibodyPlant<T>* plant_{};
};
int do_main(int argc, char* argv[]) {
gflags::ParseCommandLineFlags(&argc, &argv, true);
const Quadrotor<double> model;
auto simulator = MakeSimulatorFromGflags(model);
simulator->AdvanceTo(FLAGS_duration);
return 0;
}
} // namespace
} // namespace quadrotor
} // namespace examples
} // namespace drake
int main(int argc, char* argv[]) {
return drake::examples::quadrotor::do_main(argc, argv);
}