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.
75 lines
2.6 KiB
75 lines
2.6 KiB
#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 Acrobot, started near the upright, with an
|
|
// LQR controller designed to stabilize the unstable fixed point. Run meldis to
|
|
// see the animated result.
|
|
//
|
|
// Note: See also examples/multibody/acrobot for an almost identical test
|
|
// using the MultibodyPlant version of the Acrobot dynamics.
|
|
|
|
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 controller = builder.AddSystem(BalancingLQRController(*acrobot));
|
|
controller->set_name("controller");
|
|
builder.Connect(acrobot->get_output_port(0), controller->get_input_port());
|
|
builder.Connect(controller->get_output_port(), acrobot->get_input_port(0));
|
|
|
|
auto diagram = builder.Build();
|
|
systems::Simulator<double> simulator(*diagram);
|
|
systems::Context<double>& acrobot_context =
|
|
diagram->GetMutableSubsystemContext(*acrobot,
|
|
&simulator.get_mutable_context());
|
|
|
|
// Set an initial condition near the upright fixed point.
|
|
AcrobotState<double>* x0 = dynamic_cast<AcrobotState<double>*>(
|
|
&acrobot_context.get_mutable_continuous_state_vector());
|
|
DRAKE_DEMAND(x0 != nullptr);
|
|
x0->set_theta1(M_PI + 0.1);
|
|
x0->set_theta2(-.1);
|
|
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();
|
|
}
|