Added time stepping to examples and tests. (RobotLocomotion#7546)

drake/examples/contact_model/bowling_ball.cc

@@ -63,6 +63,11 @@ DEFINE_double(contact_area, 1e-3,
 DEFINE_double(sim_duration, 3, "The simulation duration (s)");
 DEFINE_int32(pin_count, 10, "The number of pins -- in the range [0, 10]");
 DEFINE_bool(playback, true, "If true, loops playback of simulation");
+DEFINE_string(simulation_type, "compliant", "The type of simulation to use: "
+              "'compliant' or 'timestepping'");
+DEFINE_double(dt, 1e-3, "The step size to use for "
+              "'simulation_type=timestepping' (ignored for "
+              "'simulation_type=compliant'");
 
 // Bowling ball rolled down a conceptual lane to strike pins.
 int main() {
@@ -102,7 +107,10 @@ int main() {
   multibody::AddFlatTerrainToWorld(tree_ptr.get(), 100., 10.);
 
   // Instantiate a RigidBodyPlant from the RigidBodyTree.
-  auto& plant = *builder.AddSystem<RigidBodyPlant<double>>(move(tree_ptr));
+  if (FLAGS_simulation_type != "timestepping")
+    FLAGS_dt = 0.0;
+  auto& plant = *builder.AddSystem<RigidBodyPlant<double>>(
+      move(tree_ptr), FLAGS_dt);
   plant.set_name("plant");
 
   // Note: this sets identical contact parameters across all object pairs:

drake/examples/contact_model/rigid_gripper.cc

@@ -48,6 +48,11 @@ DEFINE_double(contact_area, 1e-4,
 DEFINE_double(sim_duration, 5, "Amount of time to simulate (s)");
 DEFINE_bool(playback, true,
             "If true, simulation begins looping playback when complete");
+DEFINE_string(simulation_type, "compliant", "The type of simulation to use: "
+              "'compliant' or 'timestepping'");
+DEFINE_double(dt, 1e-3, "The step size to use for "
+              "'simulation_type=timestepping' (ignored for "
+              "'simulation_type=compliant'");
 
 namespace drake {
 namespace examples {
@@ -85,8 +90,11 @@ std::unique_ptr<RigidBodyTreed> BuildTestTree() {
 int main() {
   systems::DiagramBuilder<double> builder;
 
+  if (FLAGS_simulation_type != "timestepping")
+    FLAGS_dt = 0.0;
   systems::RigidBodyPlant<double>* plant =
-      builder.AddSystem<systems::RigidBodyPlant<double>>(BuildTestTree());
+      builder.AddSystem<systems::RigidBodyPlant<double>>(BuildTestTree(),
+      FLAGS_dt);
   plant->set_name("plant");
 
   // Command-line specified contact parameters.

drake/examples/contact_model/sliding_bricks.cc

@@ -50,6 +50,11 @@ DEFINE_double(contact_area, 1e-3,
 DEFINE_double(sim_duration, 3, "The simulation duration (s)");
 DEFINE_bool(playback, true,
             "If true, enters looping playback after sim finished");
+DEFINE_string(simulation_type, "compliant", "The type of simulation to use: "
+              "'compliant' or 'timestepping'");
+DEFINE_double(dt, 1e-3, "The step size to use for "
+              "'simulation_type=timestepping' (ignored for "
+              "'simulation_type=compliant'");
 
 namespace {
 const char* kSlidingBrickUrdf =
@@ -84,7 +89,10 @@ int main() {
   multibody::AddFlatTerrainToWorld(tree_ptr.get(), 100., 10.);
 
   // Instantiate a RigidBodyPlant from the RigidBodyTree.
-  auto& plant = *builder.AddSystem<RigidBodyPlant<double>>(move(tree_ptr));
+  if (FLAGS_simulation_type != "timestepping")
+    FLAGS_dt = 0.0;
+  auto& plant = *builder.AddSystem<RigidBodyPlant<double>>(
+      move(tree_ptr), FLAGS_dt);
   plant.set_name("plant");
 
   // Contact parameters set arbitrarily.
@@ -141,8 +149,6 @@ int main() {
       diagram->GetMutableSubsystemContext(plant, &context);
   // 6 1-dof joints * 2 = 6 * (x, ẋ) * 2
   const int kStateSize = 24;
-  DRAKE_DEMAND(plant_context.get_continuous_state_vector().size() ==
-               kStateSize);
   VectorX<double> initial_state(kStateSize);
   initial_state << 0, -0.5, 0, 0, 0, 0,  // brick 1 position
                    0, 0.5, 0, 0, 0, 0,   // brick 2 position

drake/examples/schunk_wsg/test/schunk_wsg_lift_test.cc

@@ -16,6 +16,7 @@
 #include <queue>
 #include <memory>
 
+#include <gflags/gflags.h>
 #include <gtest/gtest.h>
 
 #include "drake/common/eigen_types.h"
@@ -42,6 +43,12 @@
 #include "drake/systems/primitives/constant_vector_source.h"
 #include "drake/systems/primitives/trajectory_source.h"
 
+DEFINE_string(simulation_type, "compliant", "The type of simulation to use: "
+              "'compliant' or 'timestepping'");
+DEFINE_double(dt, 1e-3, "The step size to use for "
+              "'simulation_type=timestepping' (ignored for "
+              "'simulation_type=compliant'");
+
 namespace drake {
 namespace examples {
 namespace schunk_wsg {
@@ -172,9 +179,12 @@ GTEST_TEST(SchunkWsgLiftTest, BoxLiftTest) {
 
   int lifter_instance_id{};
   int gripper_instance_id{};
+  if (FLAGS_simulation_type != "timestepping")
+    FLAGS_dt = 0.0;
   systems::RigidBodyPlant<double>* plant =
       builder.AddSystem<systems::RigidBodyPlant<double>>(
-          BuildLiftTestTree(&lifter_instance_id, &gripper_instance_id));
+          BuildLiftTestTree(&lifter_instance_id, &gripper_instance_id),
+          FLAGS_dt);
   plant->set_name("plant");
 
   ASSERT_GE(plant->get_num_actuators(), 2);

drake/examples/valkyrie/BUILD.bazel

@@ -114,6 +114,7 @@ drake_cc_library(
         "//drake/systems/lcm:lcmt_drake_signal_translator",
         "//drake/systems/primitives:constant_vector_source",
         "//drake/systems/primitives:pass_through",
+        "@com_github_gflags_gflags//:gflags",
     ],
 )
 

drake/examples/valkyrie/test/valkyrie_simulation_test.cc

@@ -2,21 +2,32 @@
 #include "drake/examples/valkyrie/valkyrie_simulator.h"
 /* clang-format on */
 
+#include <gflags/gflags.h>
 #include <gtest/gtest.h>
 
 #include "drake/examples/valkyrie/valkyrie_constants.h"
 #include "drake/systems/analysis/semi_explicit_euler_integrator.h"
 #include "drake/systems/analysis/simulator.h"
 
+DEFINE_string(simulation_type, "compliant",
+              "Type of simulation, valid values are "
+              "'timestepping','compliant'");
+DEFINE_double(dt, 1e-3, "The step size to use for "
+              "'simulation_type=timestepping' (ignored for "
+              "'simulation_type=compliant'");
+
 namespace drake {
 namespace examples {
 namespace valkyrie {
 
 // Tests if the simulation runs at all. Nothing else.
 GTEST_TEST(ValkyrieSimulationTest, TestIfRuns) {
+  if (FLAGS_simulation_type != "timestepping")
+    FLAGS_dt = 0.0;
+
   // LCM communication.
   lcm::DrakeLcm lcm;
-  ValkyrieSimulationDiagram diagram(&lcm);
+  ValkyrieSimulationDiagram diagram(&lcm, FLAGS_dt);
 
   // Create simulator.
   systems::Simulator<double> simulator(diagram);

drake/examples/valkyrie/valkyrie_simulation.cc

@@ -4,19 +4,31 @@
 /// sends measured robot state through LCM traffic. See valkyrie_simulator.h
 /// for more details.
 
+#include <gflags/gflags.h>
+
 #include "drake/examples/valkyrie/valkyrie_constants.h"
 #include "drake/examples/valkyrie/valkyrie_simulator.h"
 #include "drake/systems/analysis/semi_explicit_euler_integrator.h"
 #include "drake/systems/analysis/simulator.h"
 
+DEFINE_string(simulation_type, "compliant",
+              "Type of simulation, valid values are "
+              "'timestepping','compliant'");
+DEFINE_double(dt, 1e-3, "The step size to use for "
+              "'simulation_type=timestepping' (ignored for "
+              "'simulation_type=compliant'");
+
 namespace drake {
 namespace examples {
 namespace valkyrie {
 
 int main() {
+  if (FLAGS_simulation_type != "timestepping")
+    FLAGS_dt = 0.0;
+
   // LCM communication.
   lcm::DrakeLcm lcm;
-  ValkyrieSimulationDiagram diagram(&lcm);
+  ValkyrieSimulationDiagram diagram(&lcm, FLAGS_dt);
 
   // Create simulator.
   systems::Simulator<double> simulator(diagram);
@@ -46,6 +58,8 @@ int main() {
 }  // namespace examples
 }  // namespace drake
 
-int main() {
+int main(int argc, char* argv[]) {
+  gflags::SetUsageMessage(" ");  // Nerf a silly warning emitted by gflags.
+  gflags::ParseCommandLineFlags(&argc, &argv, true);
   return drake::examples::valkyrie::main();
 }

drake/examples/valkyrie/valkyrie_simulator.h

@@ -36,7 +36,7 @@ namespace valkyrie {
 
 class ValkyrieSimulationDiagram : public systems::Diagram<double> {
  public:
-  explicit ValkyrieSimulationDiagram(lcm::DrakeLcm* lcm) {
+  ValkyrieSimulationDiagram(lcm::DrakeLcm* lcm, double dt) {
     systems::DiagramBuilder<double> builder;
 
     // Create RigidBodyTree.
@@ -50,8 +50,8 @@ class ValkyrieSimulationDiagram : public systems::Diagram<double> {
     multibody::AddFlatTerrainToWorld(tree_ptr.get(), 100., 10.);
 
     // Instantiate a RigidBodyPlant from the RigidBodyTree.
-    plant_ =
-        builder.AddSystem<systems::RigidBodyPlant<double>>(std::move(tree_ptr));
+    plant_ = builder.AddSystem<systems::RigidBodyPlant<double>>(
+        std::move(tree_ptr), dt);
     plant_->set_name("plant");
 
     // Contact parameters