Skip to content

Commit

Permalink
Merge pull request RobotLocomotion#6044 from siyuanfeng-tri/doc/signa…
Browse files Browse the repository at this point in the history
…llogger

Override merge for doc only change since last CI passed.
  • Loading branch information
sherm1 authored May 5, 2017
2 parents 963323c + a83871f commit adeaa3e
Show file tree
Hide file tree
Showing 2 changed files with 11 additions and 4 deletions.
6 changes: 5 additions & 1 deletion drake/systems/primitives/signal_logger.h
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,9 @@ namespace systems {

/// A sink block which logs its input to memory. This data is then retrievable
/// (e.g. after a simulation) via a handful of accessor methods.
/// This class essentially holds a large Eigen matrix for data storage, where
/// each column corresponds to a data point. This system saves a data point and
/// the context time whenever its Publish() method is called.
///
/// @tparam T The vector element type, which must be a valid Eigen scalar.
///
Expand All @@ -28,7 +31,8 @@ class SignalLogger : public LeafSystem<T> {
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SignalLogger)

/// Construct the signal logger system.
/// @param input_size Dimension of the (single) input port.
/// @param input_size Dimension of the (single) input port. This corresponds
/// to the number of rows of the data matrix.
/// @param batch_allocation_size Storage is (re)allocated in blocks of
/// input_size-by-batch_allocation_size.
explicit SignalLogger(int input_size, int batch_allocation_size = 1000);
Expand Down
9 changes: 6 additions & 3 deletions drake/systems/primitives/test/signal_logger_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -37,19 +37,22 @@ GTEST_TEST(TestSignalLogger, LinearSystemTest) {
context->get_mutable_continuous_state_vector()->SetAtIndex(0, 1.0);

simulator.Initialize();
// The Simulator internally calls Publish(), which triggers data logging.
simulator.StepTo(3);

// Gets the time stamps when each data point is saved.
const auto& t = logger->sample_times();
EXPECT_EQ(t.size(), simulator.get_num_publishes());

// Now check the data (against the known solution to the diff eq).
// Gets the logged data.
const auto& x = logger->data();
EXPECT_EQ(x.cols(), t.size());

const Eigen::MatrixXd desired_x = exp(-t.transpose().array());
// Now check the data (against the known solution to the diff eq).
const Eigen::MatrixXd expected_x = exp(-t.transpose().array());

double tol = 1e-6; // Not bad for numerical integration!
EXPECT_TRUE(CompareMatrices(desired_x, x, tol));
EXPECT_TRUE(CompareMatrices(expected_x, x, tol));
}

} // namespace
Expand Down

0 comments on commit adeaa3e

Please sign in to comment.