Merge pull request RobotLocomotion#6940 from RussTedrake/pend_cleanup

Pend cleanup

drake/examples/pendulum/BUILD

@@ -11,9 +11,17 @@ load("//tools:install_data.bzl", "install_data")
 load("//tools:lint.bzl", "add_lint_tests")
 
 drake_cc_library(
-    name = "pendulum_state_vector",
-    srcs = ["gen/pendulum_state_vector.cc"],
-    hdrs = ["gen/pendulum_state_vector.h"],
+    name = "pendulum_vector_types",
+    srcs = [
+        "gen/pendulum_input.cc",
+        "gen/pendulum_params.cc",
+        "gen/pendulum_state.cc",
+    ],
+    hdrs = [
+        "gen/pendulum_input.h",
+        "gen/pendulum_params.h",
+        "gen/pendulum_state.h",
+    ],
     deps = [
         "//drake/systems/framework:vector",
     ],
@@ -25,16 +33,17 @@ drake_cc_library(
     hdrs = ["pendulum_plant.h"],
     visibility = ["//visibility:public"],
     deps = [
-        ":pendulum_state_vector",
+        ":pendulum_vector_types",
         "//drake/systems/framework",
     ],
 )
 
 drake_cc_binary(
-    name = "pendulum_run_dynamics",
-    srcs = ["pendulum_run_dynamics.cc"],
+    name = "passive_simulation",
+    srcs = ["passive_simulation.cc"],
     add_test_rule = 1,
     data = [":models"],
+    test_rule_args = ["--target_realtime_rate=0.0"],
     deps = [
         ":pendulum_plant",
         "//drake/common:find_resource",
@@ -46,14 +55,16 @@ drake_cc_binary(
         "//drake/systems/analysis:simulator",
         "//drake/systems/framework:diagram",
         "//drake/systems/primitives:constant_vector_source",
+        "@com_github_gflags_gflags//:gflags",
     ],
 )
 
 drake_cc_binary(
-    name = "pendulum_run_energy_shaping",
-    srcs = ["pendulum_run_energy_shaping.cc"],
+    name = "energy_shaping_simulation",
+    srcs = ["energy_shaping_simulation.cc"],
     add_test_rule = 1,
     data = [":models"],
+    test_rule_args = ["--target_realtime_rate=0.0"],
     deps = [
         ":pendulum_plant",
         "//drake/common:find_resource",
@@ -64,17 +75,20 @@ drake_cc_binary(
         "//drake/systems/analysis:simulator",
         "//drake/systems/framework:diagram",
         "//drake/systems/framework:leaf_system",
+        "@com_github_gflags_gflags//:gflags",
     ],
 )
 
 drake_cc_binary(
-    name = "pendulum_run_lqr",
-    srcs = ["pendulum_run_lqr.cc"],
+    name = "lqr_simulation",
+    srcs = ["lqr_simulation.cc"],
     add_test_rule = 1,
     data = [":models"],
+    test_rule_args = ["--target_realtime_rate=0.0"],
     deps = [
         ":pendulum_plant",
         "//drake/common:find_resource",
+        "//drake/common:is_approx_equal_abstol",
         "//drake/lcm",
         "//drake/multibody/joints",
         "//drake/multibody/parsers",
@@ -83,12 +97,13 @@ drake_cc_binary(
         "//drake/systems/controllers:linear_quadratic_regulator",
         "//drake/systems/framework:diagram",
         "//drake/systems/framework:leaf_system",
+        "@com_github_gflags_gflags//:gflags",
     ],
 )
 
 drake_cc_binary(
-    name = "pendulum_run_swing_up",
-    srcs = ["pendulum_run_swing_up.cc"],
+    name = "trajectory_optimization_simulation",
+    srcs = ["trajectory_optimization_simulation.cc"],
     add_test_rule = 1,
     data = [":models"],
     test_rule_args = ["--target_realtime_rate=0.0"],
@@ -122,7 +137,7 @@ drake_cc_binary(
 # === test/ ===
 
 drake_cc_googletest(
-    name = "pendulum_urdf_dynamics_test",
+    name = "urdf_dynamics_test",
     data = ["Pendulum.urdf"],
     deps = [
         ":pendulum_plant",

drake/examples/pendulum/energy_shaping_simulation.cc

@@ -0,0 +1,123 @@
+#include <cmath>
+#include <memory>
+
+#include <gflags/gflags.h>
+
+#include "drake/common/drake_assert.h"
+#include "drake/common/find_resource.h"
+#include "drake/examples/pendulum/pendulum_plant.h"
+#include "drake/lcm/drake_lcm.h"
+#include "drake/multibody/joints/floating_base_types.h"
+#include "drake/multibody/parsers/urdf_parser.h"
+#include "drake/multibody/rigid_body_plant/drake_visualizer.h"
+#include "drake/systems/analysis/simulator.h"
+#include "drake/systems/framework/diagram.h"
+#include "drake/systems/framework/diagram_builder.h"
+#include "drake/systems/framework/leaf_system.h"
+
+namespace drake {
+namespace examples {
+namespace pendulum {
+namespace {
+
+DEFINE_double(target_realtime_rate, 1.0,
+              "Playback speed.  See documentation for "
+              "Simulator::set_target_realtime_rate() for details.");
+
+template <typename T>
+class PendulumEnergyShapingController : public systems::LeafSystem<T> {
+ public:
+  explicit PendulumEnergyShapingController(const PendulumParams<T>& params) {
+    this->DeclareVectorInputPort(PendulumState<T>());
+    this->DeclareVectorOutputPort(PendulumInput<T>(),
+                                  &PendulumEnergyShapingController::CalcTau);
+    this->DeclareNumericParameter(params);
+  }
+
+ private:
+  void CalcTau(const systems::Context<T>& context,
+               PendulumInput<T>* output) const {
+    const auto* state =
+        this->template EvalVectorInput<PendulumState>(context, 0);
+    const PendulumParams<T>& params =
+        this->template GetNumericParameter<PendulumParams>(context, 0);
+
+    // Pendulum energy shaping from Section 3.5.2 of
+    // http://underactuated.csail.mit.edu/underactuated.html?chapter=3
+    using std::pow;
+    // Desired energy is slightly more than the energy at the top (want to pass
+    // through the upright with non-zero velocity).
+    const T desired_energy =
+        1.1 * params.mass() * params.gravity() * params.length();
+    // Current total energy (see PendulumPlant::CalcTotalEnergy).
+    const T current_energy = 0.5 * params.mass() * pow(params.length(), 2) *
+                                 state->thetadot() * state->thetadot() -
+                             params.mass() * params.gravity() *
+                                 params.length() * cos(state->theta());
+    const double kEnergyFeedbackGain = .1;
+    output->set_tau(params.damping() * state->thetadot() +
+                    kEnergyFeedbackGain * state->thetadot() *
+                        (desired_energy - current_energy));
+  }
+};
+
+int do_main() {
+  lcm::DrakeLcm lcm;
+  auto tree = std::make_unique<RigidBodyTree<double>>();
+  parsers::urdf::AddModelInstanceFromUrdfFileToWorld(
+      FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"),
+      multibody::joints::kFixed, tree.get());
+
+  systems::DiagramBuilder<double> builder;
+  auto pendulum = builder.AddSystem<PendulumPlant>();
+  pendulum->set_name("pendulum");
+  // Use default pendulum parameters in the controller (real controllers never
+  // get the true parameters).
+  auto controller = builder.AddSystem<PendulumEnergyShapingController>(
+      PendulumParams<double>());
+  controller->set_name("controller");
+  builder.Connect(pendulum->get_output_port(), controller->get_input_port(0));
+  builder.Connect(controller->get_output_port(0), pendulum->get_tau_port());
+
+  auto publisher = builder.AddSystem<systems::DrakeVisualizer>(*tree, &lcm);
+  publisher->set_name("publisher");
+  builder.Connect(pendulum->get_output_port(), publisher->get_input_port(0));
+
+  auto diagram = builder.Build();
+  systems::Simulator<double> simulator(*diagram);
+  systems::Context<double>& pendulum_context =
+      diagram->GetMutableSubsystemContext(*pendulum,
+                                          simulator.get_mutable_context());
+  pendulum->set_theta(&pendulum_context, M_PI);
+  pendulum->set_thetadot(&pendulum_context, 0.0);
+
+  const double desired_energy = pendulum->CalcTotalEnergy(pendulum_context);
+
+  pendulum->set_theta(&pendulum_context, 0.1);
+  pendulum->set_thetadot(&pendulum_context, 0.2);
+
+  const double initial_energy = pendulum->CalcTotalEnergy(pendulum_context);
+
+  simulator.set_target_realtime_rate(FLAGS_target_realtime_rate);
+  simulator.Initialize();
+  simulator.StepTo(10);
+
+  const double final_energy = pendulum->CalcTotalEnergy(pendulum_context);
+
+  // Adds a numerical test that we have successfully regulated the energy
+  // towards the desired energy level.
+  DRAKE_DEMAND(std::abs(final_energy - desired_energy) <
+               (0.5 * std::abs(initial_energy - desired_energy)));
+
+  return 0;
+}
+
+}  // namespace
+}  // namespace pendulum
+}  // namespace examples
+}  // namespace drake
+
+int main(int argc, char* argv[]) {
+  gflags::ParseCommandLineFlags(&argc, &argv, true);
+  return drake::examples::pendulum::do_main();
+}

drake/examples/pendulum/gen/pendulum_input.cc

@@ -0,0 +1,23 @@
+#include "drake/examples/pendulum/gen/pendulum_input.h"
+
+// GENERATED FILE DO NOT EDIT
+// See drake/tools/lcm_vector_gen.py.
+
+namespace drake {
+namespace examples {
+namespace pendulum {
+
+const int PendulumInputIndices::kNumCoordinates;
+const int PendulumInputIndices::kTau;
+
+const std::vector<std::string>& PendulumInputIndices::GetCoordinateNames() {
+  static const never_destroyed<std::vector<std::string>> coordinates(
+      std::vector<std::string>{
+          "tau",
+      });
+  return coordinates.access();
+}
+
+}  // namespace pendulum
+}  // namespace examples
+}  // namespace drake

drake/examples/pendulum/gen/pendulum_input.h

@@ -0,0 +1,72 @@
+#pragma once
+
+// GENERATED FILE DO NOT EDIT
+// See drake/tools/lcm_vector_gen.py.
+
+#include <cmath>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "drake/common/never_destroyed.h"
+#include "drake/systems/framework/basic_vector.h"
+
+namespace drake {
+namespace examples {
+namespace pendulum {
+
+/// Describes the row indices of a PendulumInput.
+struct PendulumInputIndices {
+  /// The total number of rows (coordinates).
+  static const int kNumCoordinates = 1;
+
+  // The index of each individual coordinate.
+  static const int kTau = 0;
+
+  /// Returns a vector containing the names of each coordinate within this
+  /// class. The indices within the returned vector matches that of this class.
+  /// In other words, `PendulumInputIndices::GetCoordinateNames()[i]`
+  /// is the name for `BasicVector::GetAtIndex(i)`.
+  static const std::vector<std::string>& GetCoordinateNames();
+};
+
+/// Specializes BasicVector with specific getters and setters.
+template <typename T>
+class PendulumInput : public systems::BasicVector<T> {
+ public:
+  /// An abbreviation for our row index constants.
+  typedef PendulumInputIndices K;
+
+  /// Default constructor.  Sets all rows to zero.
+  PendulumInput() : systems::BasicVector<T>(K::kNumCoordinates) {
+    this->SetFromVector(VectorX<T>::Zero(K::kNumCoordinates));
+  }
+
+  PendulumInput<T>* DoClone() const override { return new PendulumInput; }
+
+  /// @name Getters and Setters
+  //@{
+  /// tau
+  const T& tau() const { return this->GetAtIndex(K::kTau); }
+  void set_tau(const T& tau) { this->SetAtIndex(K::kTau, tau); }
+  //@}
+
+  /// See PendulumInputIndices::GetCoordinateNames().
+  static const std::vector<std::string>& GetCoordinateNames() {
+    return PendulumInputIndices::GetCoordinateNames();
+  }
+
+  /// Returns whether the current values of this vector are well-formed.
+  decltype(T() < T()) IsValid() const {
+    using std::isnan;
+    auto result = (T(0) == T(0));
+    result = result && !isnan(tau());
+    return result;
+  }
+};
+
+}  // namespace pendulum
+}  // namespace examples
+}  // namespace drake

drake/examples/pendulum/gen/pendulum_params.cc

@@ -0,0 +1,26 @@
+#include "drake/examples/pendulum/gen/pendulum_params.h"
+
+// GENERATED FILE DO NOT EDIT
+// See drake/tools/lcm_vector_gen.py.
+
+namespace drake {
+namespace examples {
+namespace pendulum {
+
+const int PendulumParamsIndices::kNumCoordinates;
+const int PendulumParamsIndices::kMass;
+const int PendulumParamsIndices::kLength;
+const int PendulumParamsIndices::kDamping;
+const int PendulumParamsIndices::kGravity;
+
+const std::vector<std::string>& PendulumParamsIndices::GetCoordinateNames() {
+  static const never_destroyed<std::vector<std::string>> coordinates(
+      std::vector<std::string>{
+          "mass", "length", "damping", "gravity",
+      });
+  return coordinates.access();
+}
+
+}  // namespace pendulum
+}  // namespace examples
+}  // namespace drake