forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
100 lines
3.3 KiB
100 lines
3.3 KiB
#pragma once
|
|
|
|
#include "drake/common/drake_copyable.h"
|
|
#include "drake/common/eigen_types.h"
|
|
#include "drake/common/reset_after_move.h"
|
|
|
|
namespace drake {
|
|
namespace systems {
|
|
|
|
/**
|
|
This utility class serves as an in-memory cache of time-dependent vector
|
|
values. Note that this is a standalone class, not a Drake System. It is
|
|
primarily intended to support the Drake System primitive VectorLogSink, but
|
|
can be used independently.
|
|
|
|
When the log becomes full, adding more data will cause the allocated space to
|
|
double in size. If avoiding memory allocation during some performance-critical
|
|
phase is desired, clients can call Reserve() to pre-allocate log storage.
|
|
|
|
This object imposes no constraints on the stored data. For example, times
|
|
passed to AddData() need not be increasing in order of insertion, values are
|
|
allowed to be infinite, NaN, etc.
|
|
|
|
@tparam_default_scalar
|
|
*/
|
|
template <typename T>
|
|
class VectorLog {
|
|
public:
|
|
/**
|
|
The default capacity of vector log allocation, expressed as a number of
|
|
samples. Chosen to be large enough that most systems won't need to allocate
|
|
during simulation advance steps.
|
|
*/
|
|
static constexpr int64_t kDefaultCapacity = 1000;
|
|
|
|
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(VectorLog)
|
|
|
|
/** Constructs the vector log.
|
|
@param input_size Dimension of the per-time step data set.
|
|
*/
|
|
explicit VectorLog(int input_size);
|
|
|
|
/** Reports the size of the log's input vector. */
|
|
int64_t get_input_size() const { return data_.rows(); }
|
|
|
|
/** Returns the number of samples taken since construction or last Clear(). */
|
|
int num_samples() const { return num_samples_; }
|
|
|
|
// The return type here must be a VectorBlock because only the leading
|
|
// part of the sample_times_ vector contains meaningful data.
|
|
/** Accesses the logged time stamps. */
|
|
Eigen::VectorBlock<const VectorX<T>> sample_times() const {
|
|
return const_cast<const VectorX<T>&>(sample_times_).head(num_samples_);
|
|
}
|
|
|
|
/** Accesses the logged data.
|
|
|
|
The InnerPanel parameter of the return type indicates that the compiler can
|
|
assume aligned access to the data.
|
|
*/
|
|
Eigen::Block<const MatrixX<T>, Eigen::Dynamic, Eigen::Dynamic,
|
|
true /* InnerPanel */>
|
|
data() const {
|
|
return data_.leftCols(num_samples_);
|
|
}
|
|
|
|
/**
|
|
Reserve storage for at least `capacity` samples. At construction, there will
|
|
be at least `kDefaultCapacity`; use this method to reserve more.
|
|
*/
|
|
void Reserve(int64_t capacity);
|
|
|
|
/** Clears the logged data. */
|
|
void Clear() {
|
|
DRAKE_ASSERT_VOID(CheckInvariants());
|
|
// Resetting num_samples_ is sufficient to have all future writes and
|
|
// reads re-initialized to the beginning of the data.
|
|
num_samples_ = 0;
|
|
DRAKE_ASSERT_VOID(CheckInvariants());
|
|
}
|
|
|
|
/** Adds a `sample` to the data set with the associated `time` value. The new
|
|
* sample and time are added to the end of the log. No constraints are
|
|
* imposed on the values of`time` or `sample`.
|
|
|
|
@param time The time value for this sample.
|
|
@param sample A vector of data of the declared size for this log.
|
|
*/
|
|
void AddData(const T& time, const VectorX<T>& sample);
|
|
|
|
private:
|
|
void CheckInvariants() const;
|
|
|
|
reset_after_move<int64_t> num_samples_{0};
|
|
VectorX<T> sample_times_;
|
|
MatrixX<T> data_;
|
|
};
|
|
} // namespace systems
|
|
} // namespace drake
|