Drake
state.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <utility>
5 #include <vector>
6 
11 
12 namespace drake {
13 namespace systems {
14 
15 /// %State is a container for all the data comprising the
16 /// complete state of a particular System at a particular moment. Any field in
17 /// %State may be empty if it is not applicable to the System in question.
18 /// A System may not maintain state in any place other than a %State object.
19 ///
20 /// A %State `x` contains three types of state variables:
21 /// - ContinuousState `xc`
22 /// - DiscreteState `xd`
23 /// - AbstractState `xa`
24 ///
25 /// @tparam T A mathematical type compatible with Eigen's Scalar.
26 template <typename T>
27 class State {
28  public:
29  // State is not copyable or moveable.
31 
33  : abstract_state_(std::make_unique<AbstractValues>()),
34  continuous_state_(std::make_unique<ContinuousState<T>>()),
35  discrete_state_(std::make_unique<DiscreteValues<T>>()) {}
36  virtual ~State() {}
37 
38  void set_continuous_state(std::unique_ptr<ContinuousState<T>> xc) {
39  DRAKE_DEMAND(xc != nullptr);
40  continuous_state_ = std::move(xc);
41  }
42 
44  DRAKE_ASSERT(continuous_state_ != nullptr);
45  return *continuous_state_.get();
46  }
47 
49  DRAKE_ASSERT(continuous_state_ != nullptr);
50  return *continuous_state_.get();
51  }
52 
53  void set_discrete_state(std::unique_ptr<DiscreteValues<T>> xd) {
54  DRAKE_DEMAND(xd != nullptr);
55  discrete_state_ = std::move(xd);
56  }
57 
59  DRAKE_ASSERT(discrete_state_ != nullptr);
60  return *discrete_state_.get();
61  }
62 
64  DRAKE_ASSERT(discrete_state_ != nullptr);
65  return *discrete_state_.get();
66  }
67 
68  const BasicVector<T>& get_discrete_state(int index) const {
70  return xd.get_vector(index);
71  }
72 
75  return xd.get_mutable_vector(index);
76  }
77 
78  void set_abstract_state(std::unique_ptr<AbstractValues> xa) {
79  DRAKE_DEMAND(xa != nullptr);
80  abstract_state_ = std::move(xa);
81  }
82 
84  DRAKE_ASSERT(abstract_state_ != nullptr);
85  return *abstract_state_.get();
86  }
87 
89  DRAKE_ASSERT(abstract_state_ != nullptr);
90  return *abstract_state_.get();
91  }
92 
93  /// Returns a const pointer to the abstract component of the
94  /// state at @p index. Asserts if @p index doesn't exist.
95  template <typename U>
96  const U& get_abstract_state(int index) const {
97  const AbstractValues& xa = get_abstract_state();
98  return xa.get_value(index).GetValue<U>();
99  }
100 
101  /// Returns a mutable pointer to element @p index of the abstract state.
102  /// Asserts if @p index doesn't exist.
103  template <typename U>
106  return xa.get_mutable_value(index).GetMutableValue<U>();
107  }
108 
109  /// Copies the values from another State of the same scalar type into this
110  /// State.
111  void CopyFrom(const State<T>& other) {
112  continuous_state_->CopyFrom(other.get_continuous_state());
113  discrete_state_->CopyFrom(other.get_discrete_state());
114  abstract_state_->CopyFrom(other.get_abstract_state());
115  }
116 
117  /// Initializes this state (regardless of scalar type) from a State<double>.
118  /// All scalar types in Drake must support initialization from doubles.
119  void SetFrom(const State<double>& other) {
120  continuous_state_->SetFrom(other.get_continuous_state());
121  discrete_state_->SetFrom(other.get_discrete_state());
122  abstract_state_->CopyFrom(other.get_abstract_state());
123  }
124 
125  private:
126  std::unique_ptr<AbstractValues> abstract_state_;
127  std::unique_ptr<ContinuousState<T>> continuous_state_;
128  std::unique_ptr<DiscreteValues<T>> discrete_state_;
129 };
130 
131 } // namespace systems
132 } // namespace drake
AbstractValues is a container for non-numerical state and parameters.
Definition: abstract_values.h:18
DiscreteValues is a container for numerical but non-continuous state and parameters.
Definition: discrete_values.h:33
DiscreteValues< T > & get_mutable_discrete_state()
Definition: state.h:63
ContinuousState< T > & get_mutable_continuous_state()
Definition: state.h:48
Definition: automotive_demo.cc:90
STL namespace.
void set_discrete_state(std::unique_ptr< DiscreteValues< T >> xd)
Definition: state.h:53
void set_continuous_state(std::unique_ptr< ContinuousState< T >> xc)
Definition: state.h:38
void set_abstract_state(std::unique_ptr< AbstractValues > xa)
Definition: state.h:78
U & get_mutable_abstract_state(int index)
Returns a mutable pointer to element index of the abstract state.
Definition: state.h:104
const T & GetValue() const
Returns the value wrapped in this AbstractValue, which must be of exactly type T. ...
Definition: value.h:142
BasicVector< T > & get_mutable_vector()
Returns a mutable reference to the BasicVector containing the values for the only group...
Definition: discrete_values.h:112
#define DRAKE_ASSERT(condition)
DRAKE_ASSERT(condition) is similar to the built-in assert(condition) from the C++ system header <cas...
Definition: drake_assert.h:37
const U & get_abstract_state(int index) const
Returns a const pointer to the abstract component of the state at index.
Definition: state.h:96
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:45
State is a container for all the data comprising the complete state of a particular System at a parti...
Definition: state.h:27
const BasicVector< T > & get_discrete_state(int index) const
Definition: state.h:68
BasicVector is a semantics-free wrapper around an Eigen vector that satisfies VectorBase.
Definition: basic_vector.h:25
const ContinuousState< T > & get_continuous_state() const
Definition: state.h:43
const AbstractValue & get_value(int index) const
Returns the element of AbstractValues at the given index, or aborts if the index is out-of-bounds...
Definition: abstract_values.cc:33
void CopyFrom(const State< T > &other)
Copies the values from another State of the same scalar type into this State.
Definition: state.h:111
const DiscreteValues< T > & get_discrete_state() const
Definition: state.h:58
AbstractValues & get_mutable_abstract_state()
Definition: state.h:88
BasicVector< T > & get_mutable_discrete_state(int index)
Definition: state.h:73
ContinuousState is a view of, and optionally a container for, all the continuous state variables xc o...
Definition: continuous_state.h:76
void SetFrom(const State< double > &other)
Initializes this state (regardless of scalar type) from a State<double>.
Definition: state.h:119
virtual ~State()
Definition: state.h:36
AbstractValue & get_mutable_value(int index)
Returns the element of AbstractValues at the given index, or aborts if the index is out-of-bounds...
Definition: abstract_values.cc:39
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
const BasicVector< T > & get_vector() const
Returns a const reference to the BasicVector containing the values for the only group.
Definition: discrete_values.h:105
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
const AbstractValues & get_abstract_state() const
Definition: state.h:83
T & GetMutableValue()
Returns the value wrapped in this AbstractValue, which must be of exactly type T. ...
Definition: value.h:171