Merge pull request RobotLocomotion#3132 from amcastro-tri/buffer_system

Implements a PassThrough system

drake/systems/framework/CMakeLists.txt

@@ -13,6 +13,7 @@ set(sources
   primitives/constant_vector_source.cc
   primitives/gain.cc
   primitives/integrator.cc
+  primitives/pass_through.cc
   state.cc
   state_subvector.cc
   state_supervector.cc

drake/systems/framework/primitives/CMakeLists.txt

@@ -6,6 +6,8 @@ drake_install_headers(
   constant_vector_source.h
   gain.h
   gain-inl.h
-  integrator.h)
+  integrator.h
+  pass_through.h
+  pass_through-inl.h)
 
 add_subdirectory(test)

drake/systems/framework/primitives/pass_through-inl.h

@@ -0,0 +1,69 @@
+#pragma once
+
+/// @file
+/// Template method implementations for pass_through.h.
+/// Most users should only include that file, not this one.
+/// For background, see http://drake.mit.edu/cxx_inl.html.
+
+#include "drake/systems/framework/primitives/pass_through.h"
+
+#include <stdexcept>
+#include <string>
+
+#include "drake/common/drake_assert.h"
+#include "drake/drakeSystemFramework_export.h"
+#include "drake/systems/framework/basic_vector.h"
+#include "drake/systems/framework/context.h"
+
+namespace drake {
+namespace systems {
+
+template <typename T>
+PassThrough<T>::PassThrough(int length) : length_(length) {
+  // TODO(amcastro-tri): remove the length parameter from the constructor once
+  // #3109 supporting automatic lengths is resolved.
+  this->DeclareInputPort(kVectorValued, length, kInheritedSampling);
+  this->DeclareOutputPort(kVectorValued, length, kInheritedSampling);
+}
+
+template <typename T>
+void PassThrough<T>::EvalOutput(const ContextBase<T>& context,
+                          SystemOutput<T>* output) const {
+  // TODO(amcastro-tri): replace hard-coded "1" with
+  // this->get_num_output_ports() after #3097 is merged.
+  DRAKE_ASSERT(output->get_num_ports() == 1);
+  VectorInterface<T>* output_vector =
+      output->get_mutable_port(0)->GetMutableVectorData();
+  DRAKE_ASSERT(output_vector != nullptr);
+  DRAKE_ASSERT(output_vector->get_value().rows() == length_);
+
+  // Check that there are the expected number of input ports.
+  // TODO(amcastro-tri): replace hard-coded "1" with
+  // this->get_num_input_ports() after #3097 is merged.
+  if (context.get_num_input_ports() != 1) {
+    throw std::out_of_range("Expected only one input port, but had " +
+        std::to_string(context.get_num_input_ports()));
+  }
+
+  // There is only one input.
+  // TODO(amcastro-tri): Solve #3140 so that the next line reads:
+  // auto& input_vector = System<T>::get_input_vector(context, 0);
+  // where the return is an Eigen expression.
+  const VectorInterface<T>* input_vector = context.get_vector_input(0);
+
+  // Check the expected length.
+  if (input_vector == nullptr || input_vector->get_value().rows() != length_) {
+    throw std::out_of_range("Input port is nullptr or has incorrect size.");
+  }
+
+  // TODO(amcastro-tri): Solve #3140 so that we can readily access the Eigen
+  // vector like so:
+  // auto& output_vector = System<T>::get_output_vector(context, 0);
+  // where the return is an Eigen expression.
+  // TODO(amcastro-tri): the output should simply reference the input port's
+  // value to avoid copy.
+  output_vector->get_mutable_value() = input_vector->get_value();
+}
+
+}  // namespace systems
+}  // namespace drake

drake/systems/framework/primitives/pass_through.cc

@@ -0,0 +1,9 @@
+#include "drake/systems/framework/primitives/pass_through-inl.h"
+
+namespace drake {
+namespace systems {
+
+template class DRAKESYSTEMFRAMEWORK_EXPORT PassThrough<double>;
+
+}  // namespace systems
+}  // namespace drake

drake/systems/framework/primitives/pass_through.h

@@ -0,0 +1,38 @@
+#pragma once
+
+#include <memory>
+
+#include "drake/systems/framework/leaf_system.h"
+
+namespace drake {
+namespace systems {
+
+/// A pass through system with input `u` and output `y = u`. This is
+/// mathematically equivalent to a Gain system with its gain equal to one.
+/// However this system incurs no computational cost.
+/// The input to this system directly feeds through to its output.
+/// A detailed usage discussion of this system for a PID controller can be found
+/// at https://github.com/RobotLocomotion/drake/pull/3132.
+/// @tparam T The vector element type, which must be a valid Eigen scalar.
+// TODO(amcastro-tri): cross reference PidController when implemented.
+template <typename T>
+class PassThrough : public LeafSystem<T> {
+ public:
+  /// Constructs a pass thorough system (`y = u`) with input/output ports of
+  /// size @p. length.
+  /// @param length is the size of the signal to be processed.
+  explicit PassThrough(int length);
+
+  /// Sets the output port to equal the input port.
+  /// If the number of connected input or output ports is not one or the
+  /// input ports are not of size length_, `std::runtime_error` will be thrown.
+  void EvalOutput(const ContextBase<T>& context,
+                  SystemOutput<T>* output) const override;
+
+ private:
+  // TODO(amcastro-tri): remove this internal parameter once #3097 is merged.
+  const int length_;
+};
+
+}  // namespace systems
+}  // namespace drake

drake/systems/framework/primitives/test/CMakeLists.txt

@@ -2,6 +2,13 @@ add_executable(adder_test adder_test.cc)
 target_link_libraries(adder_test drakeSystemFramework ${GTEST_BOTH_LIBRARIES})
 add_test(NAME adder_test COMMAND adder_test)
 
+add_executable(pass_through_test
+        pass_through_test.cc
+        pass_through_scalartype_test.cc)
+target_link_libraries(pass_through_test drakeSystemFramework
+        ${GTEST_BOTH_LIBRARIES})
+add_test(NAME pass_through_test COMMAND pass_through_test)
+
 add_executable(integrator_test integrator_test.cc)
 target_link_libraries(integrator_test
         drakeSystemFramework ${GTEST_BOTH_LIBRARIES})

drake/systems/framework/primitives/test/pass_through_scalartype_test.cc

@@ -0,0 +1,84 @@
+#include "drake/systems/framework/primitives/pass_through-inl.h"
+
+/// @file
+/// Separate test program, so that we can use the -inl file.
+
+#include <memory>
+
+#include <unsupported/Eigen/AutoDiff>
+
+#include "drake/systems/framework/basic_vector.h"
+#include "drake/systems/framework/system_input.h"
+
+#include "gtest/gtest.h"
+
+using Eigen::AutoDiffScalar;
+using Eigen::Vector3d;
+using std::make_unique;
+
+namespace drake {
+namespace systems {
+namespace {
+
+// TODO(amcastro-tri): Create a diagram with a ConstantVectorSource feeding
+// the input of the PassThrough system.
+template<class T>
+std::unique_ptr<FreestandingInputPort<T>> MakeInput(
+    std::unique_ptr<BasicVector<T>> data) {
+  return make_unique<FreestandingInputPort<T>>(std::move(data));
+}
+
+// Tests the ability to take derivatives of the output with respect to
+// the input. Since `y = u` the derivative in this case is the identity matrix.
+GTEST_TEST(PassThroughScalarTypeTest, AutoDiff) {
+  // In this unit test with vectors of length three, derivatives will be taken
+  // with respect to the entire input vector. Therefore the Vector3d template
+  // argument.
+  typedef AutoDiffScalar<Vector3d> T;
+
+  // Set a PassThrough system with input and output of size 3.
+  auto buffer = make_unique<PassThrough<T>>(3 /* length */);
+  auto context = buffer->CreateDefaultContext();
+  auto output = buffer->AllocateOutput(*context);
+  auto input = make_unique<BasicVector<T>>(3 /* length */);
+
+  // Sets the input values.
+  Vector3<T> input_vector(1.0, 3.14, 2.18);
+
+  // Sets the input vector to be the vector of independent variables.
+  input_vector(0).derivatives() << 1, 0, 0;  // First independent variable.
+  input_vector(1).derivatives() << 0, 1, 0;  // Second independent variable.
+  input_vector(2).derivatives() << 0, 0, 1;  // Third independent variable.
+
+  input->get_mutable_value() << input_vector;
+
+  context->SetInputPort(0, MakeInput(std::move(input)));
+
+  buffer->EvalOutput(*context, output.get());
+
+  ASSERT_EQ(1, output->get_num_ports());
+  const auto& output_vector =
+      dynamic_cast<const BasicVector<T>*>(
+          output->get_port(0).get_vector_data())->get_value();
+
+  // The expected output value equals the input.
+  Vector3<T> expected;
+  expected = input_vector;
+
+  // The expected derivative is the identity matrix:
+  expected(0).derivatives() << 1.0, 0.0, 0.0;
+  expected(1).derivatives() << 0.0, 1.0, 0.0;
+  expected(2).derivatives() << 0.0, 0.0, 1.0;
+
+  for (int i = 0; i < 3; ++i) {
+    // Checks output value.
+    EXPECT_EQ(expected(i).value(), output_vector(i).value());
+
+    // Checks derivatives.
+    EXPECT_EQ(expected(i).derivatives(), output_vector(i).derivatives());
+  }
+}
+
+}  // namespace
+}  // namespace systems
+}  // namespace drake

drake/systems/framework/primitives/test/pass_through_test.cc

@@ -0,0 +1,102 @@
+#include "drake/systems/framework/primitives/pass_through.h"
+
+#include <memory>
+
+#include <unsupported/Eigen/AutoDiff>
+
+#include "drake/systems/framework/basic_vector.h"
+#include "drake/systems/framework/system_input.h"
+
+#include "gtest/gtest.h"
+
+using Eigen::AutoDiffScalar;
+using Eigen::Vector2d;
+using Eigen::Vector3d;
+using std::make_unique;
+
+namespace drake {
+namespace systems {
+namespace {
+
+// TODO(amcastro-tri): Create a diagram with a ConstantVectorSource feeding
+// the input of the PassThrough system.
+template<class T>
+std::unique_ptr<FreestandingInputPort<T>> MakeInput(
+    std::unique_ptr<BasicVector<T>> data) {
+  return make_unique<FreestandingInputPort<T>>(std::move(data));
+}
+
+class PassThroughTest : public ::testing::Test {
+ protected:
+  void SetUp() override {
+    buffer_ = make_unique<PassThrough<double>>(3 /* length */);
+    context_ = buffer_->CreateDefaultContext();
+    output_ = buffer_->AllocateOutput(*context_);
+    input_ = make_unique<BasicVector<double>>(3 /* length */);
+  }
+
+  std::unique_ptr<PassThrough<double>> buffer_;
+  std::unique_ptr<ContextBase<double>> context_;
+  std::unique_ptr<SystemOutput<double>> output_;
+  std::unique_ptr<BasicVector<double>> input_;
+};
+
+// Tests that the output of this system equals its input.
+TEST_F(PassThroughTest, VectorThroughPassThroughSystem) {
+  // Hook input of the expected size.
+  // TODO(amcastro-tri): Check with buffer_->get_num_input_ports() after #3097.
+  ASSERT_EQ(1, context_->get_num_input_ports());
+  Eigen::Vector3d input_vector(1.0, 3.14, 2.18);
+  input_->get_mutable_value() << input_vector;
+
+  context_->SetInputPort(0, MakeInput(std::move(input_)));
+
+  buffer_->EvalOutput(*context_, output_.get());
+
+  // TODO(amcastro-tri): Check with buffer_->get_num_output_ports() after #3097.
+  ASSERT_EQ(1, output_->get_num_ports());
+  const BasicVector<double>* output_vector =
+      dynamic_cast<const BasicVector<double>*>(
+          output_->get_port(0).get_vector_data());
+  ASSERT_NE(nullptr, output_vector);
+  EXPECT_EQ(input_vector, output_vector->get_value());
+}
+
+// Tests that std::out_of_range is thrown when the wrong number of input ports
+// are connected.
+TEST_F(PassThroughTest, NoInputPorts) {
+  // No input ports are hooked up. PassThrough must have one input port.
+  // TODO(amcastro-tri): This will not be needed here when input/outputs are
+  // defined in the constructor.
+  // Connections sanity check will be performed by Diagram::Finalize().
+
+  // TODO(amcastro-tri): we must be able to ask buffer_->num_of_input_ports()
+  // and make the GTest with that.
+  EXPECT_THROW(buffer_->EvalOutput(*context_, output_.get()),
+               std::out_of_range);
+}
+
+// Tests that std::out_of_range is thrown when input ports of the wrong size
+// are connected.
+// TODO(amcastro-tri): when #3109 is resolved verify that input and output ports
+// are the same size even if their sizes were determined automatically.
+TEST_F(PassThroughTest, WrongSizeOfInputPorts) {
+  // Hook up input port, but of the wrong size.
+  // TODO(amcastro-tri): Check with buffer_->get_num_input_ports() after #3097.
+  ASSERT_EQ(1, context_->get_num_input_ports());
+  auto short_input = make_unique<BasicVector<double>>(2 /* length */);
+  short_input->get_mutable_value() << 4, 5;
+  context_->SetInputPort(0, MakeInput(std::move(short_input)));
+
+  EXPECT_THROW(buffer_->EvalOutput(*context_, output_.get()),
+               std::out_of_range);
+}
+
+// Tests that PassThrough allocates no state variables in the context_.
+TEST_F(PassThroughTest, PassThroughIsStateless) {
+  EXPECT_EQ(nullptr, context_->get_state().continuous_state);
+}
+
+}  // namespace
+}  // namespace systems
+}  // namespace drake