Drake
context.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <utility>
5 
14 
15 namespace drake {
16 namespace systems {
17 
18 /// Contains information about the independent variable including time and
19 /// step number.
20 // TODO(sherm1) Add step information.
21 template <typename T>
22 struct StepInfo {
23  /// The time, in seconds. For typical T implementations based on
24  /// doubles, time resolution will gradually degrade as time increases.
25  // TODO(sherm1): Consider whether this is sufficiently robust.
26  T time_sec{0.0};
27 };
28 
29 /// %Context is an abstract class template that represents all the typed values
30 /// that are used in a System's computations: time, numeric-valued input ports,
31 /// numerical state, and numerical parameters. There are also type-erased
32 /// abstract state variables, abstract-valued input ports, abstract parameters,
33 /// and a double accuracy setting. The framework provides two concrete
34 /// subclasses of %Context: LeafContext (for leaf Systems) and DiagramContext
35 /// (for composite System Diagrams). Users are forbidden to extend
36 /// DiagramContext and are discouraged from subclassing LeafContext.
37 ///
38 /// @tparam T The mathematical type of the context, which must be a valid Eigen
39 /// scalar.
40 template <typename T>
41 class Context : public ContextBase {
42  public:
43  /// @name Does not allow copy, move, or assignment.
44  //@{
45  // Copy constructor is protected for use in implementing Clone().
46  Context(Context&&) = delete;
47  Context& operator=(const Context&) = delete;
48  Context& operator=(Context&&) = delete;
49  //@}
50 
51  /// Returns a deep copy of this Context.
52  // This is just an intentional shadowing of the base class method to return
53  // a more convenient type.
54  std::unique_ptr<Context<T>> Clone() const {
55  return dynamic_pointer_cast_or_throw<Context<T>>(ContextBase::Clone());
56  }
57 
58  ~Context() override = default;
59 
60  // =========================================================================
61  // Accessors and Mutators for Time.
62 
63  /// Returns the current time in seconds.
64  const T& get_time() const { return get_step_info().time_sec; }
65 
66  /// Set the current time in seconds.
67  virtual void set_time(const T& time_sec) {
68  step_info_.time_sec = time_sec;
69  }
70 
71  // =========================================================================
72  // Accessors and Mutators for State.
73 
74  virtual const State<T>& get_state() const = 0;
75  virtual State<T>& get_mutable_state() = 0;
76 
77  /// Returns true if the Context has no state.
78  bool is_stateless() const {
79  const int nxc = get_continuous_state().size();
80  const int nxd = get_num_discrete_state_groups();
81  const int nxa = get_num_abstract_states();
82  return nxc == 0 && nxd == 0 && nxa == 0;
83  }
84 
85  /// Returns true if the Context has continuous state, but no discrete or
86  /// abstract state.
88  const int nxc = get_continuous_state().size();
89  const int nxd = get_num_discrete_state_groups();
90  const int nxa = get_num_abstract_states();
91  return nxc > 0 && nxd == 0 && nxa == 0;
92  }
93 
94  /// Returns true if the Context has discrete state, but no continuous or
95  /// abstract state.
96  bool has_only_discrete_state() const {
97  const int nxc = get_continuous_state().size();
98  const int nxd = get_num_discrete_state_groups();
99  const int nxa = get_num_abstract_states();
100  return nxd > 0 && nxc == 0 && nxa == 0;
101  }
102 
103  /// Returns the total dimension of all of the basic vector states (as if they
104  /// were muxed).
105  /// @throws std::runtime_error if the system contains any abstract state.
106  int get_num_total_states() const {
107  DRAKE_THROW_UNLESS(get_num_abstract_states() == 0);
108  int count = get_continuous_state().size();
109  for (int i = 0; i < get_num_discrete_state_groups(); i++)
110  count += get_discrete_state(i).size();
111  return count;
112  }
113 
114  /// Sets the continuous state to @p xc, deleting whatever was there before.
115  void set_continuous_state(std::unique_ptr<ContinuousState<T>> xc) {
116  get_mutable_state().set_continuous_state(std::move(xc));
117  }
118 
119  /// Returns a mutable reference to the continuous component of the state,
120  /// which may be of size zero.
122  return get_mutable_state().get_mutable_continuous_state();
123  }
124 
125  /// Returns a mutable reference to the continuous state vector, devoid
126  /// of second-order structure. The vector may be of size zero.
128  return get_mutable_continuous_state().get_mutable_vector();
129  }
130 
131  /// Returns a const reference to the continuous component of the state,
132  /// which may be of size zero.
134  return get_state().get_continuous_state();
135  }
136 
137  /// Returns a reference to the continuous state vector, devoid of second-order
138  /// structure. The vector may be of size zero.
140  return get_continuous_state().get_vector();
141  }
142 
143  /// Returns the number of vectors (groups) in the discrete state.
145  return get_state().get_discrete_state().num_groups();
146  }
147 
148  /// Returns a reference to the entire discrete state, which may consist of
149  /// multiple discrete state vectors (groups).
151  return get_state().get_discrete_state();
152  }
153 
154  /// Returns a reference to the _only_ discrete state vector. The vector may be
155  /// of size zero.
156  /// @pre There is only one discrete state group.
158  return get_discrete_state().get_vector();
159  }
160 
161  /// Returns a mutable reference to the discrete component of the state,
162  /// which may be of size zero.
164  return get_mutable_state().get_mutable_discrete_state();
165  }
166 
167  /// Returns a mutable reference to the _only_ discrete state vector.
168  /// @sa get_discrete_state_vector().
169  /// @pre There is only one discrete state group.
171  return get_mutable_discrete_state().get_mutable_vector();
172  }
173 
174  /// Returns a mutable reference to group (vector) @p index of the discrete
175  /// state.
176  /// @pre @p index must identify an existing group.
178  DiscreteValues<T>& xd = get_mutable_discrete_state();
179  return xd.get_mutable_vector(index);
180  }
181 
182  /// Sets the discrete state to @p xd, deleting whatever was there before.
183  void set_discrete_state(std::unique_ptr<DiscreteValues<T>> xd) {
184  get_mutable_state().set_discrete_state(std::move(xd));
185  }
186 
187  /// Returns a const reference to group (vector) @p index of the discrete
188  /// state.
189  /// @pre @p index must identify an existing group.
190  const BasicVector<T>& get_discrete_state(int index) const {
191  const DiscreteValues<T>& xd = get_state().get_discrete_state();
192  return xd.get_vector(index);
193  }
194 
195  /// Returns the number of elements in the abstract state.
197  return get_state().get_abstract_state().size();
198  }
199 
200  /// Returns a const reference to the abstract component of the state, which
201  /// may be of size zero.
203  return get_state().get_abstract_state();
204  }
205 
206  /// Returns a mutable reference to the abstract component of the state,
207  /// which may be of size zero.
209  return get_mutable_state().get_mutable_abstract_state();
210  }
211 
212  /// Returns a mutable reference to element @p index of the abstract state.
213  /// @pre @p index must identify an existing element.
214  template <typename U>
216  AbstractValues& xa = get_mutable_abstract_state();
217  return xa.get_mutable_value(index).GetMutableValue<U>();
218  }
219 
220  /// Sets the abstract state to @p xa, deleting whatever was there before.
221  void set_abstract_state(std::unique_ptr<AbstractValues> xa) {
222  get_mutable_state().set_abstract_state(std::move(xa));
223  }
224 
225  /// Returns a const reference to the abstract component of the
226  /// state at @p index.
227  /// @pre @p index must identify an existing element.
228  template <typename U>
229  const U& get_abstract_state(int index) const {
230  const AbstractValues& xa = get_state().get_abstract_state();
231  return xa.get_value(index).GetValue<U>();
232  }
233 
234  // =========================================================================
235  // Accessors and Mutators for Input.
236 
237  // Allow access to the base class method (takes an AbstractValue).
239 
240  /// Connects the input port at @p index to a FixedInputPortValue with
241  /// the given vector @p vec. Aborts if @p index is out of range.
242  /// Returns a reference to the allocated FixedInputPortValue. The
243  /// reference will remain valid until this input port's value source is
244  /// replaced or the %Context is destroyed. You may use that reference to
245  /// modify the input port's value using the appropriate
246  /// FixedInputPortValue method, which will ensure that invalidation
247  /// notifications are delivered.
250  index, std::make_unique<Value<BasicVector<T>>>(vec.Clone()));
251  }
252 
253  /// Same as above method but starts with an Eigen vector whose contents are
254  /// used to initialize a BasicVector in the FixedInputPortValue.
256  int index, const Eigen::Ref<const VectorX<T>>& data) {
257  return FixInputPort(index, BasicVector<T>(data));
258  }
259 
260  /// Same as the above method that takes a `const BasicVector<T>&`, but here
261  /// the vector is passed by unique_ptr instead of by const reference. The
262  /// caller must not retain any aliases to `vec`; within this method, `vec`
263  /// is cloned and then deleted.
264  /// @note This overload will become deprecated in the future, because it can
265  /// mislead users to believe that they can retain an alias of `vec` to mutate
266  /// the fixed value during a simulation. Callers should prefer to use one of
267  /// the other overloads instead.
269  int index, std::unique_ptr<BasicVector<T>> vec) {
270  DRAKE_THROW_UNLESS(vec.get() != nullptr);
271  return FixInputPort(index, *vec);
272  }
273 
274  // =========================================================================
275  // Accessors and Mutators for Parameters.
276 
277  virtual const Parameters<T>& get_parameters() const = 0;
278  virtual Parameters<T>& get_mutable_parameters() = 0;
279 
280  /// Returns the number of vector-valued parameters.
282  return get_parameters().num_numeric_parameters();
283  }
284 
285  /// Returns a const reference to the vector-valued parameter at @p index.
286  /// Asserts if @p index doesn't exist.
287  const BasicVector<T>& get_numeric_parameter(int index) const {
288  return get_parameters().get_numeric_parameter(index);
289  }
290 
291  /// Returns a mutable reference to element @p index of the vector-valued
292  /// parameters. Asserts if @p index doesn't exist.
294  return get_mutable_parameters().get_mutable_numeric_parameter(index);
295  }
296 
297  /// Returns the number of abstract-valued parameters.
299  return get_parameters().num_abstract_parameters();
300  }
301 
302  /// Returns a const reference to the abstract-valued parameter at @p index.
303  /// Asserts if @p index doesn't exist.
304  const AbstractValue& get_abstract_parameter(int index) const {
305  return get_parameters().get_abstract_parameter(index);
306  }
307 
308  /// Returns a mutable reference to element @p index of the abstract-valued
309  /// parameters. Asserts if @p index doesn't exist.
311  return get_mutable_parameters().get_mutable_abstract_parameter(index);
312  }
313 
314  // =========================================================================
315  // Accessors and Mutators for Accuracy.
316 
317  /// Records the user's requested accuracy. If no accuracy is requested,
318  /// computations are free to choose suitable defaults, or to refuse to
319  /// proceed without an explicit accuracy setting.
320  ///
321  /// Requested accuracy is stored in the %Context for two reasons:
322  /// - It permits all computations performed over a System to see the _same_
323  /// accuracy request since accuracy is stored in one shared place, and
324  /// - it allows us to invalidate accuracy-dependent cached computations when
325  /// the requested accuracy has changed.
326  ///
327  /// The accuracy of a complete simulation or other numerical study depends on
328  /// the accuracy of _all_ contributing computations, so it is important that
329  /// each computation is done in accordance with the overall requested
330  /// accuracy. Some examples of where this is needed:
331  /// - Error-controlled numerical integrators use the accuracy setting to
332  /// decide what step sizes to take.
333  /// - The Simulator employs a numerical integrator, but also uses accuracy to
334  /// decide how precisely to isolate witness function zero crossings.
335  /// - Iterative calculations reported as results or cached internally depend
336  /// on accuracy to decide how strictly to converge the results. Examples of
337  /// these are: constraint projection, calculation of distances between
338  /// smooth shapes, and deformation calculations for soft contact.
339  ///
340  /// The common thread among these examples is that they all share the
341  /// same %Context, so by keeping accuracy here it can be used effectively to
342  /// control all accuracy-dependent computations.
343  // TODO(edrumwri) Invalidate all cached accuracy-dependent computations, and
344  // propagate accuracy to all subcontexts in a diagram context.
345  virtual void set_accuracy(const optional<double>& accuracy) {
346  accuracy_ = accuracy;
347  }
348 
349  /// Returns the accuracy setting (if any).
350  /// @see set_accuracy() for details.
351  const optional<double>& get_accuracy() const { return accuracy_; }
352 
353  // =========================================================================
354  // Miscellaneous Public Methods
355 
356  /// Returns a deep copy of this Context's State.
357  std::unique_ptr<State<T>> CloneState() const {
358  return DoCloneState();
359  }
360 
361  /// Initializes this context's time, state, and parameters from the real
362  /// values in @p source, regardless of this context's scalar type.
363  /// Requires a constructor T(double).
364  // TODO(sherm1) Should treat fixed input port values same as parameters.
366  set_time(T(source.get_time()));
367  set_accuracy(source.get_accuracy());
368  get_mutable_state().SetFrom(source.get_state());
369  get_mutable_parameters().SetFrom(source.get_parameters());
370 
371  // TODO(sherm1) Fixed input copying goes here.
372  }
373 
374  protected:
375  Context() = default;
376 
377  /// Copy constructor takes care of base class and `Context<T>` data members.
378  /// Derived classes must implement copy constructors that delegate to this
379  /// one for use in their DoCloneWithoutPointers() implementations.
380  // Default implementation invokes the base class copy constructor and then
381  // the local member copy constructors.
382  Context(const Context<T>&) = default;
383 
384  /// Clones a context but without any of its internal pointers.
385  // Structuring this as a static method permits a DiagramContext to invoke
386  // this protected functionality on its children.
387  // This is just an intentional shadowing of the base class method to return a
388  // more convenient type.
389  static std::unique_ptr<Context<T>> CloneWithoutPointers(
390  const Context<T>& source) {
391  return dynamic_pointer_cast_or_throw<Context<T>>(
393  }
394 
395  /// Override to return the appropriate concrete State class to be returned
396  /// by CloneState().
397  virtual std::unique_ptr<State<T>> DoCloneState() const = 0;
398 
399  /// Returns a const reference to current time and step information.
400  const StepInfo<T>& get_step_info() const { return step_info_; }
401 
402  private:
403  // Current time and step information.
404  StepInfo<T> step_info_;
405 
406  // Accuracy setting.
407  optional<double> accuracy_;
408 };
409 
410 } // namespace systems
411 } // namespace drake
AbstractValues is a container for non-numerical state and parameters.
Definition: abstract_values.h:18
const AbstractValue & get_abstract_parameter(int index) const
Returns a const reference to the abstract-valued parameter at index.
Definition: context.h:304
DiscreteValues< T > & get_mutable_discrete_state()
Returns a mutable reference to the discrete component of the state, which may be of size zero...
Definition: context.h:163
DiscreteValues is a container for numerical but non-continuous state and parameters.
Definition: discrete_values.h:33
const AbstractValues & get_abstract_state() const
Returns a const reference to the abstract component of the state, which may be of size zero...
Definition: context.h:202
std::unique_ptr< Context< T > > Clone() const
Returns a deep copy of this Context.
Definition: context.h:54
FixedInputPortValue & FixInputPort(int index, std::unique_ptr< BasicVector< T >> vec)
Same as the above method that takes a const BasicVector<T>&, but here the vector is passed by unique_...
Definition: context.h:268
int i
Definition: reset_after_move_test.cc:51
const BasicVector< T > & get_numeric_parameter(int index) const
Returns a const reference to the vector-valued parameter at index.
Definition: context.h:287
bool has_only_discrete_state() const
Returns true if the Context has discrete state, but no continuous or abstract state.
Definition: context.h:96
virtual const State< T > & get_state() const =0
Provides a convenient wrapper to throw an exception when a condition is unmet.
Definition: automotive_demo.cc:90
const BasicVector< T > & get_discrete_state(int index) const
Returns a const reference to group (vector) index of the discrete state.
Definition: context.h:190
void set_discrete_state(std::unique_ptr< DiscreteValues< T >> xd)
Sets the discrete state to xd, deleting whatever was there before.
Definition: context.h:183
int num_abstract_parameters() const
Returns the number of abstract-valued parameters.
Definition: context.h:298
const VectorBase< T > & get_continuous_state_vector() const
Returns a reference to the continuous state vector, devoid of second-order structure.
Definition: context.h:139
Context is an abstract class template that represents all the typed values that are used in a System&#39;...
Definition: context.h:41
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
A column vector of any size, templated on scalar type.
Definition: eigen_types.h:46
static std::unique_ptr< ContextBase > CloneWithoutPointers(const ContextBase &source)
Clones a context but without copying any of its internal pointers; the clone&#39;s pointers are set to nu...
Definition: context_base.h:238
void set_continuous_state(std::unique_ptr< ContinuousState< T >> xc)
Sets the continuous state to xc, deleting whatever was there before.
Definition: context.h:115
BasicVector< T > & get_mutable_discrete_state_vector()
Returns a mutable reference to the only discrete state vector.
Definition: context.h:170
VectorBase is an abstract base class that real-valued signals between Systems and real-valued System ...
Definition: vector_base.h:27
#define DRAKE_THROW_UNLESS(condition)
Evaluates condition and iff the value is false will throw an exception with a message showing at leas...
Definition: drake_throw.h:23
virtual const Parameters< T > & get_parameters() const =0
std::unique_ptr< BasicVector< T > > Clone() const
Copies the entire vector to a new BasicVector, with the same concrete implementation type...
Definition: basic_vector.h:130
FixedInputPortValue & FixInputPort(int index, std::unique_ptr< AbstractValue > value)
Connects the input port at index to a FixedInputPortValue with the given abstract value...
Definition: context_base.cc:54
static std::unique_ptr< Context< T > > CloneWithoutPointers(const Context< T > &source)
Clones a context but without any of its internal pointers.
Definition: context.h:389
stx::optional< T > optional
Definition: drake_optional.h:22
std::unique_ptr< ContextBase > Clone() const
Creates an identical copy of the concrete context object.
Definition: context_base.cc:11
const T & GetValue() const
Returns the value wrapped in this AbstractValue, which must be of exactly type T. ...
Definition: value.h:142
Provides non-templatized functionality shared by the templatized derived classes. ...
Definition: context_base.h:38
const BasicVector< T > & get_discrete_state_vector() const
Returns a reference to the only discrete state vector.
Definition: context.h:157
U & get_mutable_abstract_state(int index)
Returns a mutable reference to element index of the abstract state.
Definition: context.h:215
T time_sec
The time, in seconds.
Definition: context.h:26
BasicVector< T > & get_mutable_vector()
Returns a mutable reference to the BasicVector containing the values for the only group...
Definition: discrete_values.h:112
FixedInputPortValue & FixInputPort(int index, const Eigen::Ref< const VectorX< T >> &data)
Same as above method but starts with an Eigen vector whose contents are used to initialize a BasicVec...
Definition: context.h:255
void SetTimeStateAndParametersFrom(const Context< double > &source)
Initializes this context&#39;s time, state, and parameters from the real values in source, regardless of this context&#39;s scalar type.
Definition: context.h:365
virtual void set_time(const T &time_sec)
Set the current time in seconds.
Definition: context.h:67
const U & get_abstract_state(int index) const
Returns a const reference to the abstract component of the state at index.
Definition: context.h:229
int get_num_discrete_state_groups() const
Returns the number of vectors (groups) in the discrete state.
Definition: context.h:144
bool has_only_continuous_state() const
Returns true if the Context has continuous state, but no discrete or abstract state.
Definition: context.h:87
Provides drake::optional as an alias for the appropriate implementation of std::optional or std::expe...
State is a container for all the data comprising the complete state of a particular System at a parti...
Definition: state.h:27
int num_numeric_parameters() const
Returns the number of vector-valued parameters.
Definition: context.h:281
const StepInfo< T > & get_step_info() const
Returns a const reference to current time and step information.
Definition: context.h:400
const T & get_time() const
Returns the current time in seconds.
Definition: context.h:64
VectorBase< T > & get_mutable_continuous_state_vector()
Returns a mutable reference to the continuous state vector, devoid of second-order structure...
Definition: context.h:127
BasicVector is a semantics-free wrapper around an Eigen vector that satisfies VectorBase.
Definition: basic_vector.h:25
AbstractValue & get_mutable_abstract_parameter(int index)
Returns a mutable reference to element index of the abstract-valued parameters.
Definition: context.h:310
A fully type-erased container class.
Definition: value.h:101
AbstractValues & get_mutable_abstract_state()
Returns a mutable reference to the abstract component of the state, which may be of size zero...
Definition: context.h:208
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
BasicVector< T > & get_mutable_numeric_parameter(int index)
Returns a mutable reference to element index of the vector-valued parameters.
Definition: context.h:293
ContinuousState< T > & get_mutable_continuous_state()
Returns a mutable reference to the continuous component of the state, which may be of size zero...
Definition: context.h:121
Parameters is a container for variables that parameterize a System so that it can represent a family ...
Definition: parameters.h:26
int get_num_total_states() const
Returns the total dimension of all of the basic vector states (as if they were muxed).
Definition: context.h:106
BasicVector< T > & get_mutable_discrete_state(int index)
Returns a mutable reference to group (vector) index of the discrete state.
Definition: context.h:177
const optional< double > & get_accuracy() const
Returns the accuracy setting (if any).
Definition: context.h:351
virtual void set_accuracy(const optional< double > &accuracy)
Records the user&#39;s requested accuracy.
Definition: context.h:345
FixedInputPortValue & FixInputPort(int index, const BasicVector< T > &vec)
Connects the input port at index to a FixedInputPortValue with the given vector vec.
Definition: context.h:248
Contains information about the independent variable including time and step number.
Definition: context.h:22
int get_num_abstract_states() const
Returns the number of elements in the abstract state.
Definition: context.h:196
ContinuousState is a view of, and optionally a container for, all the continuous state variables xc o...
Definition: continuous_state.h:76
const DiscreteValues< T > & get_discrete_state() const
Returns a reference to the entire discrete state, which may consist of multiple discrete state vector...
Definition: context.h:150
A container class for an arbitrary type T.
Definition: value.h:252
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
A FixedInputPortValue encapsulates a vector or abstract value for use as an internal value source for...
Definition: fixed_input_port_value.h:35
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
const ContinuousState< T > & get_continuous_state() const
Returns a const reference to the continuous component of the state, which may be of size zero...
Definition: context.h:133
std::unique_ptr< State< T > > CloneState() const
Returns a deep copy of this Context&#39;s State.
Definition: context.h:357
bool is_stateless() const
Returns true if the Context has no state.
Definition: context.h:78
void set_abstract_state(std::unique_ptr< AbstractValues > xa)
Sets the abstract state to xa, deleting whatever was there before.
Definition: context.h:221
int data
Definition: value_test.cc:20
T & GetMutableValue()
Returns the value wrapped in this AbstractValue, which must be of exactly type T. ...
Definition: value.h:171