Rod2d state vector (RobotLocomotion#7379)

* Added named state to the rod.

* Added state vectors to the 2D rod.

* Lint brush.

* Lint brush.

* Lint brush (once more).

* Added tests, and fixed bugs with the named state vectors.

* Some polishing from feature review.

* Updated rod named vector file with units info.

drake/examples/rod2d/BUILD.bazel

@@ -6,10 +6,19 @@ load(
     "drake_cc_library",
     "drake_cc_binary",
 )
+load(
+    "@drake//tools/vector_gen:vector_gen.bzl",
+    "drake_cc_vector_gen_library",
+)
 load("//tools/lint:lint.bzl", "add_lint_tests")
 
 package(default_visibility = ["//visibility:public"])
 
+drake_cc_vector_gen_library(
+    name = "rod2d_state_vector",
+    srcs = ["rod2d_state_vector.named_vector"],
+)
+
 drake_cc_binary(
     name = "rod2d_sim",
     srcs = ["rod2d_sim.cc"],
@@ -39,6 +48,7 @@ drake_cc_library(
         "rod2d-inl.h",
     ],
     deps = [
+        ":rod2d_state_vector",
         "//drake/common:essential",
         "//drake/multibody/constraint",
         "//drake/solvers:mathematical_program",

drake/examples/rod2d/rod2d-inl.h

@@ -47,7 +47,7 @@ Rod2D<T>::Rod2D(SimulationType simulation_type, double dt)
 
     // Both piecewise DAE and compliant approach require six continuous
     // variables.
-    this->DeclareContinuousState(3, 3, 0);  // q, v, z
+    this->DeclareContinuousState(Rod2dStateVector<T>(), 3, 3, 0);  // q, v, z
   }
 
   this->DeclareInputPort(systems::kVectorValued, 3);

drake/examples/rod2d/rod2d.h

@@ -4,6 +4,7 @@
 #include <utility>
 #include <vector>
 
+#include "drake/examples/rod2d/gen/rod2d_state_vector.h"
 #include "drake/multibody/constraint/constraint_problem_data.h"
 #include "drake/solvers/moby_lcp_solver.h"
 #include "drake/systems/framework/leaf_system.h"
@@ -214,6 +215,28 @@ class Rod2D : public systems::LeafSystem<T> {
   ///         kPiecewiseDAE or kCompliant.
   explicit Rod2D(SimulationType simulation_type, double dt);
 
+  static const Rod2dStateVector<T>& get_state(
+      const systems::ContinuousState<T>& cstate) {
+    return dynamic_cast<const Rod2dStateVector<T>&>(cstate.get_vector());
+  }
+
+  static Rod2dStateVector<T>& get_mutable_state(
+      systems::ContinuousState<T>* cstate) {
+    return dynamic_cast<Rod2dStateVector<T>&>(cstate->get_mutable_vector());
+  }
+
+  static const Rod2dStateVector<T>& get_state(
+      const systems::Context<T>& context) {
+    return dynamic_cast<const Rod2dStateVector<T>&>(
+        context.get_continuous_state_vector());
+  }
+
+  static Rod2dStateVector<T>& get_mutable_state(
+      systems::Context<T>* context) {
+    return dynamic_cast<Rod2dStateVector<T>&>(
+        context->get_mutable_continuous_state_vector());
+  }
+
   /// Gets the constraint force mixing parameter (CFM, used for time stepping
   /// systems only).
   double get_cfm() const { return cfm_; }

drake/examples/rod2d/rod2d_state_vector.named_vector

@@ -0,0 +1,32 @@
+namespace: "drake::examples::rod2d"
+
+element {
+     name: "x"
+     doc: "Horizontal location of the center-of-mass."
+     doc_units: "m"
+}
+element {
+     name: "y"
+     doc: "Vertical location of the center-of-mass."
+     doc_units: "m"
+}
+element {
+     name: "theta"
+     doc: "Angle of the rod (measured counter-clockwise)."
+     doc_units: "rad"
+}
+element {
+     name: "xdot"
+     doc: "Velocity of the horizontal location of the center-of-mass."
+     doc_units: "m/s"
+}
+element {
+     name: "ydot"
+     doc: "Velocity of the vertical location of the center-of-mass."
+     doc_units: "m/s"
+}
+element {
+     name: "thetadot"
+     doc: "Angular velocity of the rod."
+     doc_units: "rad/s"
+}

drake/examples/rod2d/test/rod2d_test.cc

@@ -284,6 +284,37 @@ class Rod2DDAETest : public ::testing::Test {
       contact_solver_;
 };
 
+// Verifies that the state vector functions throw no exceptions.
+TEST_F(Rod2DDAETest, NamedStateVectorsNoThrow) {
+  EXPECT_NO_THROW(Rod2D<double>::get_mutable_state(context_.get()));
+  EXPECT_NO_THROW(Rod2D<double>::get_state(*context_));
+  EXPECT_NO_THROW(Rod2D<double>::get_state(context_->get_continuous_state()));
+  EXPECT_NO_THROW(Rod2D<double>::get_mutable_state(
+      &context_->get_mutable_continuous_state()));
+}
+
+// Tests that named state vector components are at expected indices.
+TEST_F(Rod2DDAETest, ExpectedIndices) {
+  // Set the state.
+  Rod2dStateVector<double>& state = Rod2D<double>::get_mutable_state(
+      context_.get());
+  state.set_x(1.0);
+  state.set_y(2.0);
+  state.set_theta(3.0);
+  state.set_xdot(5.0);
+  state.set_ydot(7.0);
+  state.set_thetadot(11.0);
+
+  // Check the indices.
+  const VectorBase<double>& x = context_->get_continuous_state_vector();
+  EXPECT_EQ(state.x(), x[0]);
+  EXPECT_EQ(state.y(), x[1]);
+  EXPECT_EQ(state.theta(), x[2]);
+  EXPECT_EQ(state.xdot(), x[3]);
+  EXPECT_EQ(state.ydot(), x[4]);
+  EXPECT_EQ(state.thetadot(), x[5]);
+}
+
 // Checks that the output port represents the state.
 TEST_F(Rod2DDAETest, Output) {
   const ContinuousState<double>& xc = context_->get_continuous_state();