-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
diagram_continuous_state.cc
65 lines (54 loc) · 2.24 KB
/
diagram_continuous_state.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
#include "drake/systems/framework/diagram_continuous_state.h"
#include <utility>
#include "drake/common/pointer_cast.h"
#include "drake/systems/framework/supervector.h"
namespace drake {
namespace systems {
template <typename T>
DiagramContinuousState<T>::DiagramContinuousState(
std::vector<ContinuousState<T>*> substates)
: ContinuousState<T>(
Span(substates, x_selector), Span(substates, q_selector),
Span(substates, v_selector), Span(substates, z_selector)),
substates_(std::move(substates)) {
DRAKE_ASSERT(internal::IsNonNull(substates_));
}
template <typename T>
DiagramContinuousState<T>::DiagramContinuousState(
std::vector<std::unique_ptr<ContinuousState<T>>> substates)
: DiagramContinuousState<T>(internal::Unpack(substates)) {
owned_substates_ = std::move(substates);
DRAKE_ASSERT(internal::IsNonNull(owned_substates_));
}
template <typename T>
DiagramContinuousState<T>::~DiagramContinuousState() {}
template <typename T>
std::unique_ptr<DiagramContinuousState<T>>
DiagramContinuousState<T>::Clone() const {
// We are sure of the type here because DoClone() is final. However,
// we'll still use the `..._or_throw` spelling as a sanity check.
return dynamic_pointer_cast_or_throw<DiagramContinuousState>(
ContinuousState<T>::Clone());
}
template <typename T>
std::unique_ptr<ContinuousState<T>> DiagramContinuousState<T>::DoClone() const {
std::vector<std::unique_ptr<ContinuousState<T>>> owned_states;
// Make deep copies regardless of whether they were owned.
for (auto state : substates_) owned_states.push_back(state->Clone());
return std::make_unique<DiagramContinuousState>(std::move(owned_states));
}
template <typename T>
std::unique_ptr<VectorBase<T>> DiagramContinuousState<T>::Span(
const std::vector<ContinuousState<T>*>& substates,
std::function<VectorBase<T>&(ContinuousState<T>*)> selector) {
std::vector<VectorBase<T>*> sub_xs;
for (const auto& substate : substates) {
DRAKE_DEMAND(substate != nullptr);
sub_xs.push_back(&selector(substate));
}
return std::make_unique<Supervector<T>>(sub_xs);
}
} // namespace systems
} // namespace drake
DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
class ::drake::systems::DiagramContinuousState)