forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
87 lines
2.7 KiB
87 lines
2.7 KiB
2 years ago
|
/// @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);
|
||
|
|
}
|